| 3,6 → 3,10 |
|---|
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // + |
| // + GPS read out: |
| // + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany |
| // + only for non-profit use |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| #include "main.h" |
| 34,42 → 38,136 |
|---|
| const unsigned char ANALOG_TEXT[32][16] = |
| { |
| //1234567890123456 |
| "IntegralNick ", //0 |
| "IntegralRoll ", |
| "AccNick ", |
| "AccRoll ", |
| "GyroGier ", |
| "HoehenWert ", //5 |
| "AccZ ", |
| "Gas ", |
| "KompassValue ", |
| "Spannung ", |
| "Empfang ", //10 |
| "Analog11 ", |
| "Motor_Vorne ", |
| "Motor_Hinten ", |
| "Motor_Links ", |
| "Motor_Rechts ", //15 |
| "Acc_Z ", |
| "MittelAccNick ", |
| "MittelAccRoll ", |
| "IntegralErrNick ", |
| "IntegralErrRoll ", //20 |
| "MittelIntNick ", |
| "MittelIntRoll ", |
| "NeutralNick ", |
| "RollOffset ", |
| "IntRoll*Faktor ", //25 |
| "Analog26 ", |
| "DirektAusglRoll ", |
| "MesswertRoll ", |
| "AusgleichRoll ", |
| "I-LageRoll ", //30 |
| "StickRoll " |
| "IntegralNick____", //00 |
| "IntegralRoll____", //01 |
| "AccNick_________", //02 |
| "AccRoll_________", //03 |
| "UserParam1______", //04 |
| "UserParam2______", //05 |
| "Soll_Pos_North__", //06 |
| "Soll_Pos_East___", //07 |
| "KompassValue____", //08 |
| "UBat____________", //09 |
| "SenderOkay______", //10 |
| "GPSFix__________", //11 |
| "Gesamtstrom_____", //12 |
| "Geschwindigkeit_", //13 |
| "Pos.north_______", //14 |
| "Pos.east________", //15 |
| "Poti3___________", //16 |
| "Höhe_GPS________", //17 |
| "Home_North______", //18 |
| "Home_East_______", //19 |
| "Posabw_North____", //20 |
| "Posabw_East_____", //21 |
| "P_Einfluss_North", //22 |
| "P_Einfluss_East_", //23 |
| "D_Einfluss_North", //24 |
| "D_Einfluss_East_", //25 |
| "NORTH_MITTEL____", //26 |
| "EAST_MITTEL_____", //27 |
| "GPS_Nick________", //28 |
| "GPS_Roll________", //29 |
| "StickNick_______", //30 |
| "StickRoll_______" //31 |
| }; |
| |
| |
| char newData_navPosUtm = 0; // Flag, wenn neue PosUtm-Daten vorliegen (211007Kr) |
| |
| static uint8_t gpsState; |
| #define GPS_EMPTY 0 |
| #define GPS_SYNC1 1 |
| #define GPS_SYNC2 2 |
| #define GPS_CLASS 3 |
| #define GPS_LEN1 4 |
| #define GPS_LEN2 5 |
| #define GPS_FILLING 6 |
| #define GPS_CKA 7 |
| #define GPS_CKB 8 |
| |
| gpsInfo_t actualPos; // measured position (last gps record) |
| |
| #define SYNC_CHAR1 0xb5 |
| #define SYNC_CHAR2 0x62 |
| |
| #define CLASS_NAV 0x01 |
| #define MSGID_STATUS 0x03 |
| //#define MSGID_POSLLH 0x02 //(231107Kr) |
| #define MSGID_POSUTM 0x08 |
| #define MSGID_VELNED 0x12 |
| |
| |
| |
| typedef struct { |
| unsigned long ITOW; // 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 |
| unsigned long TTFF; // Time to first fix (millisecond time tag) |
| unsigned long MSSS; // Milliseconds since Startup / Reset |
| uint8_t packetStatus; |
| } NAV_STATUS_t; |
| |
| /* |
| typedef struct { //(231107Kr) |
| unsigned long ITOW; // time of week |
| long LON; // longitude in 1e-07 deg |
| long LAT; // lattitude |
| long HEIGHT; // height in mm |
| long HMSL; // height above mean sea level im mm |
| unsigned long Hacc; // horizontal accuracy in mm |
| unsigned long Vacc; // vertical accuracy in mm |
| uint8_t packetStatus; |
| } NAV_POSLLH_t; |
| */ |
| |
| typedef struct { |
| unsigned long ITOW; // time of week |
| long EAST; // cm UTM Easting |
| long NORTH; // cm UTM Nording |
| long ALT; // cm altitude |
| uint8_t ZONE; // UTM zone number |
| uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
| uint8_t packetStatus; |
| } NAV_POSUTM_t; |
| |
| |
| typedef struct { |
| unsigned long ITOW; // ms GPS Millisecond Time of Week |
| long VEL_N; // cm/s NED north velocity |
| long VEL_E; // cm/s NED east velocity |
| long VEL_D; // cm/s NED down velocity |
| unsigned long Speed; // cm/s Speed (3-D) |
| unsigned long GSpeed; // cm/s Ground Speed (2-D) |
| long Heading; // deg (1e-05) Heading 2-D |
| unsigned long SAcc; // cm/s Speed Accuracy Estimate |
| unsigned long CAcc; // deg Course / Heading Accuracy Estimate |
| uint8_t packetStatus; |
| } NAV_VELNED_t; |
| |
| |
| |
| NAV_STATUS_t navStatus; |
| //NAV_POSLLH_t navPosLlh; //(231107Kr) |
| NAV_POSUTM_t navPosUtm; |
| NAV_VELNED_t navVelNed; |
| |
| volatile char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
| volatile uint8_t CK_A, CK_B; // UBX checksum bytes |
| volatile unsigned short msgLen; |
| volatile uint8_t msgID; |
| volatile uint8_t ignorePacket; // true when previous packet was not processed |
| |
| |
| #ifdef GPS_DEBUG // if set then the GPS data is transfered to display |
| extern volatile uint8_t v24state; |
| char buf[200]; |
| char *bp; |
| char *ep; |
| #endif |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++ Sende-Part der Datenübertragung |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 91,6 → 189,51 |
|---|
| else ptr = 0; |
| } |
| |
| void GPSscanData (void) |
| { |
| |
| |
| |
| |
| if (navStatus.packetStatus == 1) // valid packet |
| { |
| actualPos.GPSFix = navStatus.GPSfix; |
| actualPos.newData = navStatus.packetStatus; |
| navStatus.packetStatus = 0; |
| } |
| |
| /* |
| if (navPosLlh.packetStatus == 1) // valid packet |
| { |
| actualPos.longi = navPosLlh.LON; //(231107Kr) |
| actualPos.lati = navPosLlh.LAT; //(231107Kr) |
| actualPos.height = navPosLlh.HEIGHT; //(231107Kr) |
| navPosLlh.packetStatus = 0; |
| } |
| */ |
| |
| if (navPosUtm.packetStatus == 1) // valid packet |
| { |
| actualPos.northing = navPosUtm.NORTH; |
| actualPos.easting = navPosUtm.EAST; |
| actualPos.altitude = navPosUtm.ALT; |
| navPosUtm.packetStatus = 0; |
| newData_navPosUtm = 1; // (211007Kr) |
| ROT_ON; // Rot blinkt in der Frequenz mit der neue UTM-Daten des GPS Empfänger ankommen und brauchbar sind //(211107Kr) |
| } |
| |
| |
| if (navVelNed.packetStatus == 1) // valid packet |
| { |
| actualPos.velNorth = navVelNed.VEL_N; |
| actualPos.velEast = navVelNed.VEL_E; |
| actualPos.velDown = navVelNed.VEL_D; |
| actualPos.GSpeed = navVelNed.GSpeed; //Geschwindigkeit [cm/s] über Grund (151007Kr) |
| navVelNed.packetStatus = 0; |
| } |
| |
| } |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 102,6 → 245,137 |
|---|
| unsigned char CrcOkay = 0; |
| |
| SioTmp = UDR; |
| |
| uint8_t c; |
| uint8_t re; |
| |
| re = (UCSR0A & (_B1(FE0) | _B1(DOR0))); // any error occured ? |
| c = SioTmp; |
| |
| #ifdef GPS_DEBUG |
| *bp++ = c; |
| if (bp >= (buf+200)) bp = buf; |
| if (v24state == 0) |
| { |
| v24state = 1; |
| UDR0 = *ep++; |
| if (ep >= buf+200) |
| ep = buf; |
| UCSR0B |= _B1(UDRIE0); //enable further irqs |
| } |
| #endif |
| |
| 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: //(231107Kr) |
| 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 |
| 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; |
| } |
| GPSscanData (); |
| |
| |
| |
| |
| |
| |
| |
| if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0; |
| if(SioTmp == '\r' && UartState == 2) |
| { |
| 165,7 → 439,6 |
|---|
| } |
| |
| |
| |
| // -------------------------------------------------------------------------- |
| void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
| { |
| 342,6 → 615,8 |
|---|
| //fdevopen (uart_putchar, 0); |
| //sbi(PORTD,4); |
| Debug_Timer = SetDelay(200); |
| |
| gpsState = GPS_EMPTY; |
| } |
| |
| //--------------------------------------------------------------------------------------------- |