12,61 → 12,74 |
unless it is stated otherwise. |
*/ |
|
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "main.h" |
#include "kafi.h" |
|
#include "mymath.h" |
#include <math.h> |
|
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
|
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
|
int GPSTracking = 0; |
int targetPosValid = 0; |
int homePosValid = 0; |
|
uint8_t gpsState; |
|
gpsInfo_t actualPos; // measured position (last gps record) |
gpsInfo_t targetPos; // measured position (last gps record) |
gpsInfo_t homePos; // measured position (last gps record) |
int holdPosValid = 0; |
|
NAV_STATUS_t navStatus; |
NAV_POSLLH_t navPosLlh; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
|
volatile gpsInfo_t actualPos;// measured position (last gps record) |
volatile gpsInfo_t targetPos;// measured position (last gps record) |
volatile gpsInfo_t homePos;// measured position (last gps record) |
volatile gpsInfo_t holdPos; // measured position (last gps record) |
|
NAV_STATUS_t navStatus; |
NAV_POSLLH_t navPosLlh; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
|
uint8_t gpsState; |
|
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
|
|
long distanceNS = 0; |
long distanceEW = 0; |
long distanceEW = 0; |
long velocityNS = 0; |
long velocityEW = 0; |
unsigned long maxDistance = 0; |
|
int roll_gain = 0; |
int nick_gain = 0; |
int nick_gain_p = 0; |
int nick_gain_d = 0; |
int roll_gain_p = 0; |
int roll_gain_d = 0; |
int nick_gain = 0; |
int nick_gain_p, nick_gain_d; |
int roll_gain_p, roll_gain_d; |
int Override = 0; |
int TargetGier = 0; |
|
int TargetGier = 0; |
|
extern int sollGier; |
|
char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
uint8_t CK_A, CK_B; // UBX checksum bytes |
uint8_t ignorePacket; // true when previous packet was not processed |
unsigned short msgLen; |
uint8_t msgID; |
extern int RemoteLinkLost; |
|
volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered |
volatile uint8_t CK_A, CK_B;// UBX checksum bytes |
volatile uint8_t ignorePacket;// true when previous packet was not processed |
volatile unsigned short msgLen; |
volatile uint8_t msgID; |
|
void GPSscanData (void); |
void GPSupdate(void); |
void SetNewHeading(unsigned long maxDistance); |
|
|
/* **************************************************************************** |
Functionname: GPSupdate */ /*! |
Description: |
|
@param[in] |
|
@return void |
@pre - |
@post - |
74,208 → 87,271 |
**************************************************************************** */ |
void GPSupdate(void) |
{ |
float SIN_H, COS_H; |
long max_p = 0; |
long max_d = 0; |
|
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 15) |
{ |
Override = 1; |
} |
else |
{ |
Override = 0; |
} |
float SIN_H, COS_H; |
long max_p = 0; |
long max_d = 0; |
int SwitchPos = 0; |
|
/* Determine Selector Switch Position */ |
if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100) |
{ |
SwitchPos = 0; |
} |
else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) |
{ |
SwitchPos = 2;/* Target Mode */ |
} |
else |
{ |
SwitchPos = 1;/* Position Hold */ |
} |
|
/* Overide On / Off */ |
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10) |
{ |
Override = 1; |
} |
else |
{ |
Override = 0; |
} |
|
/* Set Home Position */ |
if ((actualPos.state > 2) && |
(homePosValid == 0)) |
{ |
homePos.north = actualPos.north; |
homePos.east = actualPos.east; |
homePos.altitude = actualPos.altitude ; |
homePosValid = 1; |
} |
|
/* Set Target Position */ |
if ((actualPos.state > 2) && |
(SwitchPos == 0)) |
{ |
targetPos.north = actualPos.north; |
targetPos.east = actualPos.east; |
targetPos.altitude = actualPos.altitude ; |
targetPosValid = 1; |
} |
if ((actualPos.state < 3) && |
(SwitchPos == 0)) |
{ |
targetPosValid = 0; |
} |
|
/* Set Hold Position */ |
if ((actualPos.state > 2) && |
((Override == 1) || |
(SwitchPos == 2) )) /* Update the hold position in case we are in target mode */ |
{ |
holdPos.north = actualPos.north; |
holdPos.east = actualPos.east; |
holdPos.altitude = actualPos.altitude ; |
holdPosValid = 1; |
} |
if ((actualPos.state < 3) && |
(Override == 1)) |
{ |
holdPosValid = 0; |
} |
|
/* Indicate Valid GPS Position */ |
if ((actualPos.state > 2) && |
(SwitchPos == 0)) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 5000; |
BeepMuster = 0x0100; |
} |
} |
|
if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */ |
{ |
max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
} |
else |
{ |
max_p = 8; |
max_d = 4; |
} |
|
/* Those seem to be good values */ |
max_p = 8; |
max_d = 4; |
|
//DebugOut.Analog[11] = max_p; |
//DebugOut.Analog[12] = max_d; |
|
static int GPSTrackingCycles = 0; |
long maxGainPos = 140; |
long maxGainVel = 140; |
|
/* Ramp up gain */ |
if (GPSTrackingCycles < 1000) |
{ |
GPSTrackingCycles++; |
} |
maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
|
/* Determine Offset from nominal Position */ |
if (actualPos.state > 2 ) |
{ |
if ((SwitchPos == 2) && |
(targetPosValid == 1) && |
(RemoteLinkLost == 0) && |
(Override == 0)) |
{ /* determine distance from target position */ |
distanceNS = actualPos.north - targetPos.north; // in 0.1m |
distanceEW = actualPos.east - targetPos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
} |
else if ((SwitchPos == 1)&& |
(holdPosValid == 1) && |
(RemoteLinkLost == 0) && |
(Override == 0)) |
{ /* determine distance from hold position */ |
distanceNS = actualPos.north - holdPos.north; // in 0.1m |
distanceEW = actualPos.east - holdPos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
} |
else if ((RemoteLinkLost == 1) && |
(homePosValid ==1)) /* determine distance from target position */ |
{/* Overide in case the remote link got lost */ |
distanceNS = actualPos.north - homePos.north; // in 0.1m |
distanceEW = actualPos.east - homePos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
} |
else |
{ |
distanceNS = 0.0F; // in 0.1m |
distanceEW = 0.0F; // in 0.1m |
velocityNS = 0.0F; // in 0.1m/s |
velocityEW = 0.0F; // in 0.1m/s |
maxDistance = 0.0F; |
GPSTrackingCycles = 0; |
} |
|
/* Limit GPS_Nick to 25 */ |
nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS * max_d)/50))); //m/s |
roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50))); //m/s |
TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
|
/* PD-Control */ |
nick_gain = nick_gain_p + nick_gain_d; |
roll_gain = -(roll_gain_p + roll_gain_d); |
|
/* Calculate Distance to Target */ |
SIN_H = (float) sin(status.Psi); |
COS_H =(float) cos(status.Psi); |
|
GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
|
GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
|
/* Set New Heading */ |
SetNewHeading(maxDistance); |
} |
else |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
maxDistance = 0.0F; |
} |
} |
|
/* Set Home Position */ |
if ((actualPos.state > 2) && |
(homePosValid == 0)) |
{ |
/* Save Current Position */ |
homePos.north = actualPos.north; |
homePos.east = actualPos.east; |
homePos.altitude = actualPos.altitude ; |
homePosValid = 1; |
} |
|
/* Set Target Position */ |
if ((actualPos.state > 2) && |
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)) |
{ |
/* Save Current Position */ |
targetPos.north = actualPos.north; |
targetPos.east = actualPos.east; |
targetPos.altitude = actualPos.altitude ; |
targetPosValid = 1; |
|
if(BeepMuster == 0xffff) |
{ |
beeptime = 5000; |
BeepMuster = 0x0100; |
} |
} |
|
if ((GPSTracking == 1) && |
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100)) |
{ |
GPSTracking = 0; |
beeptime = 5000; |
BeepMuster = 0x0080; |
} |
|
/* The System is in Tracking State*/ |
if ((GPSTracking == 0) && |
(targetPosValid == 1) && |
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) && |
(actualPos.state > 2)) |
{ |
GPSTracking = 1; |
beeptime = 5000; |
BeepMuster = 0x0c00; |
} |
|
max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
|
#if 0 |
DebugOut.Analog[11] = max_p; |
DebugOut.Analog[12] = max_d; |
if (maxDistance / 10 < 12) |
{ |
static int iState = 0; |
switch (iState) |
{ |
case 0: |
targetPos.north += 400; |
targetPos.east += 0; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 1: |
targetPos.north += 0; |
targetPos.east += 400; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 2: |
targetPos.north -= 400; |
targetPos.east += 0; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 3: |
targetPos.north += 0; |
targetPos.east -= 400; |
GPSTrackingCycles = 0; |
iState=0; |
break; |
default: |
iState=0; |
break; |
} |
beeptime = 5000; |
BeepMuster = 0x0c00; |
} |
#endif |
|
static int GPSTrackingCycles = 0; |
long maxGainPos = 140; |
long maxGainVel = 140; |
|
/* Slowly ramp up gain */ |
if (GPSTrackingCycles < 1000) |
{ |
GPSTrackingCycles++; |
} |
maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
|
if (actualPos.state > 2 ) |
{ |
distanceNS = actualPos.north - targetPos.north; // in 0.1m |
distanceEW = actualPos.east - targetPos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
|
// Limit GPS_Nick to 25 |
nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s |
roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s |
|
TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
} |
|
if ((GPSTracking == 1) && |
(actualPos.state > 2) && |
(Override == 0)) |
{ |
/* Calculate Distance to Target */ |
SIN_H = (float) sin(status.Psi); |
COS_H =(float) cos(status.Psi); |
|
/* PD-Regler */ |
nick_gain = nick_gain_p + nick_gain_d; |
roll_gain = -(roll_gain_p + roll_gain_d); |
|
GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
|
GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
|
/* Turn the mikrokopter slowly towards the target position */ |
{ |
int OffsetGier; |
if (maxDistance / 10 > 2) |
{ |
OffsetGier = 2; |
} |
else |
{ |
OffsetGier = 0; |
} |
|
if (TargetGier < 0) |
{ |
TargetGier += 3600; |
} |
if (TargetGier > sollGier) |
{ |
if (TargetGier - sollGier < 1800) |
{ |
sollGier += OffsetGier; |
} |
else |
{ |
sollGier -= OffsetGier; |
} |
} |
else |
{ |
if (sollGier - TargetGier < 1800) |
{ |
sollGier -= OffsetGier; |
} |
else |
{ |
sollGier += OffsetGier; |
} |
} |
} |
#if 0 |
/* Go round in a square */ |
if (maxDistance / 10 < 10) |
{ |
static int iState = 0; |
switch (iState) |
{ |
case 0: |
targetPos.north += 400; |
targetPos.east += 0; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 1: |
targetPos.north += 0; |
targetPos.east += 400; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 2: |
targetPos.north -= 400; |
targetPos.east += 0; |
GPSTrackingCycles = 0; |
iState++; |
break; |
case 3: |
targetPos.north += 0; |
targetPos.east -= 400; |
GPSTrackingCycles = 0; |
iState=0; |
break; |
default: |
iState=0; |
break; |
} |
beeptime = 5000; |
BeepMuster = 0x0c00; |
} |
#endif |
} |
else |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
GPSTrackingCycles = 0; |
} |
void SetNewHeading(unsigned long maxDistance) |
/* Set New Heading */ |
{ |
int OffsetGier = 0; |
if (maxDistance / 10 > 8) |
{ |
OffsetGier = 4; |
} |
|
if (TargetGier < 0) |
{ |
TargetGier += 3600; |
} |
if (TargetGier > sollGier) |
{ |
if (TargetGier - sollGier < 1800) |
{ |
sollGier += OffsetGier; |
} |
else |
{ |
sollGier -= OffsetGier; |
} |
} |
else |
{ |
if (sollGier - TargetGier < 1800) |
{ |
sollGier -= OffsetGier; |
} |
else |
{ |
sollGier += OffsetGier; |
} |
} |
} |
|
/* **************************************************************************** |
289,68 → 365,68 |
**************************************************************************** */ |
void InitGPS(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); |
|
// 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 = 0; |
UBRR1L = BAUDRATE; |
|
// enable double speed operation |
//UCSR1A |= (1 << U2X1); |
UCSR1A = _B0(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; |
|
gpsState = GPS_EMPTY; |
// 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); |
|
// 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 = 0; |
UBRR1L = BAUDRATE; |
|
// enable double speed operation |
//UCSR1A |= (1 << U2X1); |
UCSR1A = _B0(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; |
|
gpsState = GPS_EMPTY; |
} |
|
|
365,41 → 441,41 |
**************************************************************************** */ |
void GPSscanData (void) |
{ |
if (navStatus.packetStatus == 1) // valid packet |
{ |
actualPos.state = navStatus.GPSfix; |
if ((actualPos.state > 1) && (GPSTracking == 0)) |
{ |
GRN_FLASH; |
} |
navStatus.packetStatus = 0; |
} |
|
if (navPosUtm.packetStatus == 1) // valid packet |
{ |
actualPos.north = navPosUtm.NORTH/10L; |
actualPos.east = navPosUtm.EAST/10L; |
actualPos.altitude = navPosUtm.ALT/100; |
actualPos.ITOW = navPosUtm.ITOW; |
navPosUtm.packetStatus = 0; |
} |
/* |
if (navPosLlh.packetStatus == 1) |
{ |
actualPos.longi = navPosLlh.LON; |
actualPos.lati = navPosLlh.LAT; |
actualPos.height = navPosLlh.HEIGHT; |
navPosLlh.packetStatus = 0; |
} |
*/ |
if (navVelNed.packetStatus == 1) |
{ |
actualPos.velNorth = navVelNed.VEL_N; |
actualPos.velEast = navVelNed.VEL_E; |
actualPos.velDown = navVelNed.VEL_D; |
actualPos.groundSpeed = navVelNed.GSpeed; |
navVelNed.packetStatus = 0; |
} |
if (navStatus.packetStatus == 1)// valid packet |
{ |
actualPos.state = navStatus.GPSfix; |
if ((actualPos.state > 1) && (GPSTracking == 0)) |
{ |
GRN_FLASH; |
} |
navStatus.packetStatus = 0; |
} |
|
if (navPosUtm.packetStatus == 1)// valid packet |
{ |
actualPos.north = navPosUtm.NORTH/10L; |
actualPos.east = navPosUtm.EAST/10L; |
actualPos.altitude = navPosUtm.ALT/100; |
actualPos.ITOW = navPosUtm.ITOW; |
navPosUtm.packetStatus = 0; |
} |
/* |
if (navPosLlh.packetStatus == 1) |
{ |
actualPos.longi = navPosLlh.LON; |
actualPos.lati = navPosLlh.LAT; |
actualPos.height = navPosLlh.HEIGHT; |
navPosLlh.packetStatus = 0; |
} |
*/ |
if (navVelNed.packetStatus == 1) |
{ |
actualPos.velNorth = navVelNed.VEL_N; |
actualPos.velEast = navVelNed.VEL_E; |
actualPos.velDown = navVelNed.VEL_D; |
actualPos.groundSpeed = navVelNed.GSpeed; |
navVelNed.packetStatus = 0; |
} |
} |
|
/* **************************************************************************** |
413,133 → 489,133 |
**************************************************************************** */ |
SIGNAL (SIG_USART1_RECV) |
{ |
uint8_t c; |
uint8_t re; |
|
re = (UCSR1A & (_B1(FE1) | _B1(DOR1))); // any error occured ? |
c = UDR1; |
|
if (re == 0) |
{ |
switch (gpsState) |
{ |
case GPS_EMPTY: |
if (c == SYNC_CHAR1) |
{ |
gpsState = GPS_SYNC1; |
} |
break; |
case GPS_SYNC1: |
if (c == SYNC_CHAR2) |
{ |
gpsState = GPS_SYNC2; |
} |
else if (c != SYNC_CHAR1) |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_SYNC2: |
if (c == CLASS_NAV) |
{ |
gpsState = GPS_CLASS; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CLASS: // msg ID seen: init packed receive |
msgID = c; |
CK_A = CLASS_NAV + c; |
CK_B = CLASS_NAV + CK_A; |
gpsState = GPS_LEN1; |
|
switch (msgID) |
{ |
case MSGID_STATUS: |
ubxP = (char*)&navStatus; |
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
ubxSp = (char*)&navStatus.packetStatus; |
ignorePacket = navStatus.packetStatus; |
break; |
/* |
case MSGID_POSLLH: |
ubxP = (char*)&navPosLlh; |
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
ubxSp = (char*)&navPosLlh.packetStatus; |
ignorePacket = navPosLlh.packetStatus; |
break; |
*/ |
case MSGID_POSUTM: |
ubxP = (char*)&navPosUtm; |
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
ubxSp = (char*)&navPosUtm.packetStatus; |
ignorePacket = navPosUtm.packetStatus; |
break; |
case MSGID_VELNED: |
ubxP = (char*)&navVelNed; |
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
ubxSp = (char*)&navVelNed.packetStatus; |
ignorePacket = navVelNed.packetStatus; |
break; |
default: |
ignorePacket = 1; |
ubxSp = (char*)0; |
} |
break; |
case GPS_LEN1: // first len byte |
msgLen = c; |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_LEN2; |
break; |
case GPS_LEN2: // second len byte |
msgLen = msgLen + (c * 256); |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_FILLING; // next data will be stored in packet struct |
break; |
case GPS_FILLING: |
CK_A += c; |
CK_B += CK_A; |
|
if ( !ignorePacket && ubxP < ubxEp) |
{ |
*ubxP++ = c; |
} |
|
if (--msgLen == 0) |
{ |
gpsState = GPS_CKA; |
} |
break; |
case GPS_CKA: |
if (c == CK_A) |
{ |
gpsState = GPS_CKB; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CKB: |
if (c == CK_B && ubxSp) // No error -> packet received successfully |
{ |
*ubxSp = 1; // set packetStatus in struct |
GPSscanData(); |
} |
gpsState = GPS_EMPTY; // ready for next packet |
break; |
default: |
gpsState = GPS_EMPTY; // ready for next packet |
} |
} |
else // discard any data if error occured |
{ |
gpsState = GPS_EMPTY; |
} |
uint8_t c; |
uint8_t re; |
|
re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ? |
c = UDR1; |
|
if (re == 0) |
{ |
switch (gpsState) |
{ |
case GPS_EMPTY: |
if (c == SYNC_CHAR1) |
{ |
gpsState = GPS_SYNC1; |
} |
break; |
case GPS_SYNC1: |
if (c == SYNC_CHAR2) |
{ |
gpsState = GPS_SYNC2; |
} |
else if (c != SYNC_CHAR1) |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_SYNC2: |
if (c == CLASS_NAV) |
{ |
gpsState = GPS_CLASS; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CLASS:// msg ID seen: init packed receive |
msgID = c; |
CK_A = CLASS_NAV + c; |
CK_B = CLASS_NAV + CK_A; |
gpsState = GPS_LEN1; |
|
switch (msgID) |
{ |
case MSGID_STATUS: |
ubxP = (char*)&navStatus; |
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
ubxSp = (char*)&navStatus.packetStatus; |
ignorePacket = navStatus.packetStatus; |
break; |
/* |
case MSGID_POSLLH: |
ubxP = (char*)&navPosLlh; |
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
ubxSp = (char*)&navPosLlh.packetStatus; |
ignorePacket = navPosLlh.packetStatus; |
break; |
*/ |
case MSGID_POSUTM: |
ubxP = (char*)&navPosUtm; |
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
ubxSp = (char*)&navPosUtm.packetStatus; |
ignorePacket = navPosUtm.packetStatus; |
break; |
case MSGID_VELNED: |
ubxP = (char*)&navVelNed; |
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
ubxSp = (char*)&navVelNed.packetStatus; |
ignorePacket = navVelNed.packetStatus; |
break; |
default: |
ignorePacket = 1; |
ubxSp = (char*)0; |
} |
break; |
case GPS_LEN1:// first len byte |
msgLen = c; |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_LEN2; |
break; |
case GPS_LEN2:// second len byte |
msgLen = msgLen + (c * 256); |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_FILLING;// next data will be stored in packet struct |
break; |
case GPS_FILLING: |
CK_A += c; |
CK_B += CK_A; |
|
if ( !ignorePacket && ubxP < ubxEp) |
{ |
*ubxP++ = c; |
} |
|
if (--msgLen == 0) |
{ |
gpsState = GPS_CKA; |
} |
break; |
case GPS_CKA: |
if (c == CK_A) |
{ |
gpsState = GPS_CKB; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CKB: |
if (c == CK_B && ubxSp)// No error -> packet received successfully |
{ |
*ubxSp = 1;// set packetStatus in struct |
GPSscanData(); |
} |
gpsState = GPS_EMPTY;// ready for next packet |
break; |
default: |
gpsState = GPS_EMPTY;// ready for next packet |
} |
} |
else// discard any data if error occured |
{ |
gpsState = GPS_EMPTY; |
} |
} |
|
|