| 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; |
| } |
| } |
| |
| |