/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 |