| /branches/V0.68d Code Redesign killagreg/fifo.c |
|---|
| 0,0 → 1,28 |
| #include "fifo.h" |
| void fifo_init (fifo_t *f, uint8_t *buffer, const uint8_t size) |
| { |
| f->count = 0; |
| f->pread = f->pwrite = buffer; |
| f->read2end = f->write2end = f->size = size; |
| } |
| uint8_t fifo_put (fifo_t *f, const uint8_t data) |
| { |
| return _inline_fifo_put (f, data); |
| } |
| uint8_t fifo_get_wait (fifo_t *f) |
| { |
| while (!f->count); |
| return _inline_fifo_get (f); |
| } |
| int16_t fifo_get_nowait (fifo_t *f) |
| { |
| if (!f->count) return -1; |
| return (int16_t) _inline_fifo_get (f); |
| } |
| /branches/V0.68d Code Redesign killagreg/fifo.h |
|---|
| 0,0 → 1,98 |
| #ifndef _FIFO_H_ |
| #define _FIFO_H_ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| // the fifo object |
| typedef struct |
| { |
| uint8_t volatile count; // # number of characters in FIFO |
| uint8_t size; // buffer size |
| uint8_t *pread; // read pointer |
| uint8_t *pwrite; // write pointer |
| uint8_t read2end, write2end; // number of characters for buffer overflow for read/write pointers |
| } fifo_t; |
| /* |
| The initialization of the FIFO sets the read/write pointers etc.. |
| The FIFO uses the buffer 'buf' which byte length must 'size'. |
| */ |
| extern void fifo_init (fifo_t*, uint8_t* buf, const uint8_t size); |
| /* |
| Puts a byte into the FIFO. Returns 1 on success ans 0 in case of FIFO overflow. |
| */ |
| extern uint8_t fifo_put (fifo_t*, const uint8_t data); |
| /* |
| Get the next byte out of the FIFO. If the FIFO is empty the function blocks |
| until the next byte is put into the FIFO. |
| */ |
| extern uint8_t fifo_get_wait (fifo_t*); |
| /* |
| Get the next byte from the FIFO as int. Returns -1 if the FIFO is empty. |
| */ |
| extern int16_t fifo_get_nowait (fifo_t*); |
| /* |
| The same like fifo_put |
| */ |
| static inline uint8_t _inline_fifo_put (fifo_t *f, const uint8_t data) |
| { |
| if (f->count >= f->size) |
| return 0; |
| uint8_t * pwrite = f->pwrite; |
| *(pwrite++) = data; |
| uint8_t write2end = f->write2end; |
| if (--write2end == 0) |
| { |
| write2end = f->size; |
| pwrite -= write2end; |
| } |
| f->write2end = write2end; |
| f->pwrite = pwrite; |
| uint8_t sreg = SREG; |
| cli(); |
| f->count++; |
| SREG = sreg; |
| return 1; |
| } |
| /* |
| Get the next byte from FIFO. Before this functionis called |
| it must be checked that there is a byte in the FIFO to get. |
| */ |
| static inline uint8_t _inline_fifo_get (fifo_t *f) |
| { |
| uint8_t *pread = f->pread; |
| uint8_t data = *(pread++); |
| uint8_t read2end = f->read2end; |
| if (--read2end == 0) |
| { |
| read2end = f->size; |
| pread -= read2end; |
| } |
| f->pread = pread; |
| f->read2end = read2end; |
| uint8_t sreg = SREG; |
| cli(); |
| f->count--; |
| SREG = sreg; |
| return data; |
| } |
| #endif /* _FIFO_H_ */ |
| /branches/V0.68d Code Redesign killagreg/mymath.c |
|---|
| 0,0 → 1,78 |
| #include <avr/pgmspace.h> |
| #include "mymath.h" |
| // discrete mathematics |
| // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
| int16_t c_sin_8192(int16_t angle) |
| { |
| int8_t m,n; |
| int16_t sinus; |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = -angle; |
| } |
| else m = 1; |
| // fold angle to intervall 0 to 359 |
| angle %= 360; |
| // queck quadrant |
| if (angle <= 90) n=1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = pgm_read_word(&pgm_sinlookup[angle]); |
| // calculate sinus value |
| return (sinus * m * n); |
| } |
| // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| int16_t c_cos_8192(int16_t angle) |
| { |
| return (c_sin_8192(90 - angle)); |
| } |
| // Arcustangens returns degree in a range of +/. 180 deg |
| const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
| int16_t c_atan2(int16_t y, int16_t x) |
| { |
| int16_t index, angle; |
| int8_t m; |
| if (!x && !y) return 0; //atan2(0, 0) is undefined |
| if (y < 0) m = -1; |
| else m = 1; |
| if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
| index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
| if (index < 0) index = -index; |
| if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
| else if (index > 7334) angle = 90; // limit is 90 deg |
| else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
| else if (index > 1465) angle = 88; |
| else if (index > 1046) angle = 87; |
| else if (index > 813) angle = 86; |
| else if (index > 664) angle = 85; |
| else if (index > 561) angle = 84; |
| else if (index > 486) angle = 83; |
| else if (index > 428) angle = 82; |
| else if (index > 382) angle = 81; |
| else angle = 80; // (index>345) |
| if (x > 0) return (angle * m); // 1st and 4th quadrant |
| else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
| else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
| } |
| /branches/V0.68d Code Redesign killagreg/mymath.h |
|---|
| 0,0 → 1,8 |
| #ifndef _MYMATH_H |
| #define _MYMATH_H |
| extern int16_t c_sin_8192(int16_t angle); |
| extern int16_t c_cos_8192(int16_t angle); |
| extern int16_t c_atan2(signed int y, signed int x); |
| #endif // _MYMATH_H |
| /branches/V0.68d Code Redesign killagreg/timer2.c |
|---|
| 0,0 → 1,101 |
| #include "main.h" |
| int16_t ServoValue = 0; |
| /*****************************************************/ |
| /* Initialize Timer 2 */ |
| /*****************************************************/ |
| // The timer 2 is used to generate the PWM at PD7 (J7) |
| // to control a camera servo for nick compensation |
| void TIMER2_Init(void) |
| { |
| uint8_t sreg = SREG; |
| // disable all interrupts before reconfiguration |
| cli(); |
| // set PD7 as output for the PWM |
| DDRD |=(1<<DDD7); |
| PORTB |= (1<<PORTD7); |
| // Timer/Counter 2 Control Register A |
| // Waveform Generation Mode is Fast PWM (Bits WGM22 = 0, WGM21 = 1, WGM20 = 1) |
| // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits COM2A1 = 1, COM2A0 = 0) |
| // PD6: Normal port operation, OC2B disconnected, (Bits COM2B1 = 0, COM2B0 = 0) |
| TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0)); |
| TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20); |
| // Timer/Counter 2 Control Register B |
| // set clock devider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz |
| // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
| // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
| // devider 8 (Bits CS022 = 0, CS21 = 1, CS20 = 0) |
| TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22)); |
| TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20); |
| // initialize the Output Compare Register A used for PWM generation on port PD7 |
| OCR2A = 10; |
| // init Timer/Counter 2 Register |
| TCNT2 = 0; |
| // Timer/Counter 2 Interrupt Mask Register |
| // enable timer output compare match A Interrupt only |
| TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
| TIMSK2 |= (1<<OCIE2A); |
| SREG = sreg; |
| } |
| /*****************************************************/ |
| /* Control Servo Position */ |
| /*****************************************************/ |
| ISR(TIMER2_COMPA_vect) // 9,765 kHz |
| { |
| static uint8_t timer = 10; |
| if(!timer--) |
| { |
| // enable PWM on PD7 in non inverting mode |
| TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0); |
| ServoValue = Parameter_ServoNickControl; |
| // inverting movment of servo |
| if(EE_Parameter.ServoNickCompInvert & 0x01) |
| { |
| ServoValue += ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
| } |
| else // non inverting movment of servo |
| { |
| ServoValue -= ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
| } |
| // limit servo value to its parameter range definition |
| if(ServoValue < EE_Parameter.ServoNickMin) |
| { |
| ServoValue = EE_Parameter.ServoNickMin; |
| } |
| else if(ServoValue > EE_Parameter.ServoNickMax) |
| { |
| ServoValue = EE_Parameter.ServoNickMax; |
| } |
| // update PWM |
| OCR2A = ServoValue; |
| timer = EE_Parameter.ServoNickRefresh; |
| } |
| else |
| { |
| // disable PWM at PD7 |
| TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)); |
| // set PD7 to GND |
| PORTD &= ~(1<<PORTD7); |
| } |
| } |
| /branches/V0.68d Code Redesign killagreg/timer2.h |
|---|
| 0,0 → 1,11 |
| #ifndef _TIMER2_H |
| #define _TIMER2_H |
| extern int16_t ServoValue; |
| void TIMER2_Init(void); |
| #endif //_TIMER2_H |
| /branches/V0.68d Code Redesign killagreg/uart1.c |
|---|
| 0,0 → 1,141 |
| #include "main.h" |
| #include "uart1.h" |
| #include "fifo.h" |
| #include "ubx.h" |
| // FIFO-objects and buffers for input and output |
| //#define BUFSIZE_IN 0x96 |
| //volatile uint8_t inbuf[BUFSIZE_IN]; |
| //fifo_t infifo; |
| #define BUFSIZE_OUT 0x96 |
| volatile uint8_t outbuf[BUFSIZE_OUT]; |
| fifo_t outfifo; |
| /****************************************************************/ |
| /* Initialization of the USART1 */ |
| /****************************************************************/ |
| void USART1_Init (void) |
| { |
| // USART1 Control and Status Register A, B, C and baud rate register |
| uint8_t sreg = SREG; |
| uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
| // disable all interrupts before reconfiguration |
| cli(); |
| // disable RX-Interrupt |
| UCSR1B &= ~(1 << RXCIE1); |
| // disable TX-Interrupt |
| UCSR1B &= ~(1 << TXCIE1); |
| // disable DRE-Interrupt |
| UCSR1B |= (1 << UDRIE1); |
| // disable receiver and transmitter (will flush the buffers) |
| UCSR1B &= ~((1 << TXEN1) | (1 << RXEN1)); |
| // set direction of RXD1 and TXD1 pins |
| // set RXD1 (PD2) as an input pin |
| PORTD |= (1 << PORTD2); |
| DDRD &= ~(1 << DDD2); |
| // set TXD1 (PD3) as an output pin |
| PORTD |= (1 << PORTD3); |
| DDRD |= (1 << DDD3); |
| // USART0 Baud Rate Register |
| // set clock divider |
| UBRR1H = (uint8_t)(ubrr>>8); |
| UBRR1L = (uint8_t)ubrr; |
| // enable double speed operation |
| UCSR1A |= (1 << U2X1); |
| // enable receiver and transmitter |
| UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
| // set asynchronous mode |
| UCSR1C &= ~(1 << UMSEL11); |
| UCSR1C &= ~(1 << UMSEL10); |
| // no parity |
| UCSR1C &= ~(1 << UPM11); |
| UCSR1C &= ~(1 << UPM10); |
| // 1 stop bit |
| UCSR1C &= ~(1 << USBS1); |
| // 8-bit |
| UCSR1B &= ~(1 << UCSZ12); |
| UCSR1C |= (1 << UCSZ11); |
| UCSR1C |= (1 << UCSZ10); |
| // flush receive buffer explicit |
| while ( UCSR1A & (1<<RXC1) ) UDR1; |
| // enable interrupts at the end |
| // enable RX-Interrupt |
| UCSR1B |= (1 << RXCIE1); |
| // enable TX-Interrupt |
| //UCSR1B |= (1 << TXCIE1); |
| // enable DRE interrupt |
| //UCSR1B |= (1 << UDRIE1); |
| // restore global interrupt flags |
| SREG = sreg; |
| // inint FIFO buffer |
| //fifo_init (&infifo, inbuf, BUFSIZE_IN); |
| fifo_init (&outfifo, outbuf, BUFSIZE_OUT); |
| } |
| int16_t USART1_putc (const uint8_t c) |
| { |
| int16_t ret = fifo_put (&outfifo, c); |
| // create an data register empty interrupt |
| UCSR1B |= (1 << UDRIE1); |
| return ret; |
| } |
| /*int16_t USART1_getc_nowait () |
| { |
| return fifo_get_nowait (&infifo); |
| } |
| uint8_t USART1_getc_wait () |
| { |
| return fifo_get_wait (&infifo); |
| } |
| */ |
| /****************************************************************/ |
| /* USART1 data register empty ISR */ |
| /****************************************************************/ |
| ISR(USART1_UDRE_vect) |
| { |
| // Move a character from the output buffer to the data register. |
| // When the character was processed the next interrupt is generated. |
| // If the output buffer is empty the DRE-interrupt is disabled. |
| if (outfifo.count > 0) |
| UDR1 = _inline_fifo_get (&outfifo); |
| else |
| UCSR1B &= ~(1 << UDRIE1); |
| } |
| /****************************************************************/ |
| /* USART1 transmitter ISR */ |
| /****************************************************************/ |
| ISR(USART1_TX_vect) |
| { |
| } |
| /****************************************************************/ |
| /* USART1 receiver ISR */ |
| /****************************************************************/ |
| ISR(USART1_RX_vect) |
| { |
| uint8_t c; |
| c = UDR0; // get data byte |
| ubx_parser(c); // and put it into the ubx protocol parser |
| } |
| /branches/V0.68d Code Redesign killagreg/uart1.h |
|---|
| 0,0 → 1,25 |
| #ifndef _UART1_H |
| #define _UART1_H |
| #define USART1_BAUD 57600 |
| /* |
| Initialize the USART und activate the receiver and transmitter |
| as well as the receive-interrupt. The IO-FIFOs are initialized. |
| The global interrupt-enable-flag (I-Bit in SREG) is not changed |
| */ |
| extern void USART1_Init (void); |
| /* |
| The character c is stored in the output buffer. If the character was pushed sucessfully to |
| the output buffer then the return value is 1. In case of an output buffer overflow the return value is 0. |
| The isr is activated, which will send the data from the outbut buffer to the UART. |
| */ |
| extern int USART1_putc (const uint8_t c); |
| /* |
| extern uint8_t USART1_getc_wait(void); |
| extern int16_t USART1_getc_nowait(void); |
| */ |
| #endif //_UART1_H |
| /branches/V0.68d Code Redesign killagreg/ubx.c |
|---|
| 0,0 → 1,221 |
| #include <inttypes.h> |
| #include "ubx.h" |
| // ubx protocol parser state machine |
| #define UBXSTATE_IDLE 0 |
| #define UBXSTATE_SYNC1 1 |
| #define UBXSTATE_SYNC2 2 |
| #define UBXSTATE_CLASS 3 |
| #define UBXSTATE_LEN1 4 |
| #define UBXSTATE_LEN2 5 |
| #define UBXSTATE_DATA 6 |
| #define UBXSTATE_CKA 7 |
| #define UBXSTATE_CKB 8 |
| // ublox protocoll identifier |
| #define UBX_CLASS_NAV 0x01 |
| #define UBX_ID_POSLLH 0x02 |
| #define UBX_ID_POSUTM 0x08 |
| #define UBX_ID_STATUS 0x03 |
| #define UBX_ID_VELNED 0x12 |
| #define UBX_SYNC1_CHAR 0xB5 |
| #define UBX_SYNC2_CHAR 0x62 |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| uint8_t GPSfix; // GPSfix Type, range 0..6 |
| uint8_t Flags; // Navigation Status Flags |
| uint8_t DiffS; // Differential Status |
| uint8_t res; // reserved |
| uint32_t TTFF; // Time to first fix (millisecond time tag) |
| uint32_t MSSS; // Milliseconds since Startup / Reset |
| uint8_t packetStatus; |
| } GPS_STATUS_t; |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t LON; // 1e-07 deg Longitude |
| int32_t LAT; // 1e-07 deg Latitude |
| int32_t HEIGHT; // mm Height above Ellipsoid |
| int32_t HMSL; // mm Height above mean sea level |
| uint32_t Hacc; // mm Horizontal Accuracy Estimate |
| uint32_t Vacc; // mm Vertical Accuracy Estimate |
| uint8_t packetStatus; |
| } GPS_POSLLH_t; |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t EAST; // cm UTM Easting |
| int32_t NORTH; // cm UTM Nording |
| int32_t ALT; // cm altitude |
| uint8_t ZONE; // UTM zone number |
| uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
| uint8_t packetStatus; |
| } GPS_POSUTM_t; |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t VEL_N; // cm/s NED north velocity |
| int32_t VEL_E; // cm/s NED east velocity |
| int32_t VEL_D; // cm/s NED down velocity |
| uint32_t Speed; // cm/s Speed (3-D) |
| uint32_t GSpeed; // cm/s Ground Speed (2-D) |
| int32_t Heading; // 1e-05 deg Heading 2-D |
| uint32_t SAcc; // cm/s Speed Accuracy Estimate |
| uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
| uint8_t packetStatus; |
| } GPS_VELNED_t; |
| GPS_STATUS_t GpsStatus; |
| GPS_POSLLH_t GpsPosLlh; |
| GPS_POSUTM_t GpsPosUtm; |
| GPS_VELNED_t GpsVelNed; |
| GPS_INFO_t actualPos; |
| void UpdateGPSInfo (void) |
| { |
| if (GpsStatus.packetStatus == VALID) // valid packet |
| { |
| actualPos.satfix = GpsStatus.GPSfix; |
| GpsStatus.packetStatus = PROCESSED; // never update old data |
| } |
| if (GpsPosLlh.packetStatus == VALID) // valid packet |
| { |
| actualPos.longitude = GpsPosLlh.LON; |
| actualPos.latitude = GpsPosLlh.LAT; |
| actualPos.altitude = GpsPosLlh.HEIGHT; |
| GpsPosLlh.packetStatus = PROCESSED; // never update old data |
| } |
| if (GpsPosUtm.packetStatus == VALID) // valid packet |
| { |
| actualPos.utmeast = GpsPosUtm.EAST; |
| actualPos.utmnorth = GpsPosUtm.NORTH; |
| actualPos.utmalt = GpsPosUtm.ALT; |
| GpsPosUtm.packetStatus = PROCESSED; // never update old data |
| } |
| if (GpsVelNed.packetStatus == VALID) // valid packet |
| { |
| actualPos.veleast = GpsVelNed.VEL_E; |
| actualPos.velnorth = GpsVelNed.VEL_N; |
| actualPos.veltop = -GpsVelNed.VEL_D; |
| GpsPosUtm.packetStatus = PROCESSED; // never update old data |
| } |
| } |
| // this function should be called within the UART RX ISR |
| void ubx_parser(uint8_t c) |
| { |
| static uint8_t ubxstate = UBXSTATE_IDLE; |
| static uint8_t cka, ckb; |
| static uint16_t msglen; |
| static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
| switch(ubxstate) |
| { |
| case UBXSTATE_IDLE: // check 1st sync byte |
| if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1; |
| else ubxstate = UBXSTATE_IDLE; // out of synchronization |
| break; |
| case UBXSTATE_SYNC1: // check 2nd sync byte |
| if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2; |
| else ubxstate = UBXSTATE_IDLE; // out of synchronization |
| break; |
| case UBXSTATE_SYNC2: // check msg class to be NAV |
| if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS; |
| else ubxstate = UBXSTATE_IDLE; // unsupported message class |
| break; |
| case UBXSTATE_CLASS: // check message identifier |
| switch(c) |
| { |
| case UBX_ID_POSUTM: // utm position |
| ubxP = (int8_t *)&GpsPosUtm; // data start pointer |
| ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer |
| ubxSp = (int8_t *)&GpsPosUtm.packetStatus; // status pointer |
| break; |
| case UBX_ID_POSLLH: // geodetic position |
| ubxP = (int8_t *)&GpsStatus; // data start pointer |
| ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
| ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer |
| break; |
| case UBX_ID_STATUS: // receiver status |
| ubxP = (int8_t *)&GpsStatus; // data start pointer |
| ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
| ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer |
| break; |
| case UBX_ID_VELNED: // velocity vector in tangent plane |
| ubxP = (int8_t *)&GpsVelNed; // data start pointer |
| ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer |
| ubxSp = (int8_t *)&GpsVelNed.packetStatus; // status pointer |
| break; |
| default: // unsupported identifier |
| ubxstate = UBXSTATE_IDLE; |
| break; |
| } |
| if (ubxstate != UBXSTATE_IDLE) |
| { |
| ubxstate = UBXSTATE_LEN1; |
| cka = UBX_CLASS_NAV + c; |
| ckb = UBX_CLASS_NAV + cka; |
| } |
| break; |
| case UBXSTATE_LEN1: // 1st message length byte |
| msglen = c; |
| cka += c; |
| ckb += cka; |
| ubxstate = UBXSTATE_LEN2; |
| break; |
| case UBXSTATE_LEN2: // 2nd message length byte |
| msglen += ((uint16_t)c)<<8; |
| cka += c; |
| ckb += cka; |
| // if the old data are not processed so far then break parsing now |
| // to avoid writing new data in ISR during reading by another function |
| if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE; |
| else // data invalid or allready processd |
| { |
| *ubxSp = INVALID; |
| ubxstate = UBXSTATE_DATA; |
| } |
| break; |
| case UBXSTATE_DATA: |
| if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left |
| cka += c; |
| ckb += cka; |
| if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read |
| break; |
| case UBXSTATE_CKA: |
| if (c == cka) ubxstate = UBXSTATE_CKB; |
| else ubxstate = UBXSTATE_IDLE; |
| break; |
| case UBXSTATE_CKB: |
| if (c == ckb) |
| { |
| *ubxSp = VALID; // new data are availabe |
| UpdateGPSInfo(); //update GPS info respectively |
| } |
| ubxstate = UBXSTATE_IDLE; // ready to parse new data |
| break; |
| default: // unknown ubx state |
| ubxstate = UBXSTATE_IDLE; |
| break; |
| } |
| } |
| /branches/V0.68d Code Redesign killagreg/ubx.h |
|---|
| 0,0 → 1,45 |
| #ifndef _UBX_H |
| #define _UBX_H |
| #define INVALID 0x00 |
| #define VALID 0x01 |
| #define PROCESSED 0x02 |
| #define SATFIX_NONE 0x00 |
| #define SATFIX_DEADRECKOING 0x01 |
| #define SATFIX_2D 0x02 |
| #define SATFIX_3D 0x03 |
| #define SATFIX_GPS_DEADRECKOING 0x04 |
| #define SATFIX_TIMEONLY 0x05 |
| /* enable the UBX protocol at the gps receiver with the following messages enabled |
| 01-02 NAV - POSLLH |
| 01-03 NAV - STATUS |
| 01-08 NAV - POSUTM |
| 01-12 NAV - VELNED */ |
| typedef struct |
| { |
| uint8_t satfix; // type of satfix |
| int32_t utmnorth; // in cm (+ = north) |
| int32_t utmeast; // in cm (+ = east) |
| int32_t utmalt; // in cm (+ = top) |
| int32_t velnorth; // in cm/s |
| int32_t veleast; // in cm/s |
| int32_t veltop; // in cm/s |
| int32_t longitude; // in 1e-07 deg |
| int32_t latitude; // in 1e-07 deg |
| int32_t altitude; // in mm |
| uint8_t status; // status of data: invalid | valid |
| } GPS_INFO_t; |
| //here you will find the current gps info |
| extern GPS_INFO_t actualPos; // measured position (last gps record) |
| // this function should be called within the UART RX ISR |
| extern void ubx_parser(uint8_t c); |
| #endif //_UBX_H |