WebSVN - FlightCtrl - Diff - Rev 36 and 37 - /branches/GPS_BETA_chris2798


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + 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"
#include "uart.h"
 
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
unsigned volatile char SioTmp = 0;
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NeuerDatensatzEmpfangen = 0;
unsigned volatile char NeueKoordinateEmpfangen = 0;
unsigned volatile char UebertragungAbgeschlossen = 1;
unsigned volatile char CntCrcError = 0;
unsigned volatile char AnzahlEmpfangsBytes = 0;
unsigned volatile char PC_DebugTimeout = 0;
unsigned char PcZugriff = 100;
unsigned char MotorTest[4] = {0,0,0,0};
unsigned char MeineSlaveAdresse;
struct str_DebugOut    DebugOut;
struct str_Debug       DebugIn;
struct str_VersionInfo VersionInfo;
int Debug_Timer;
 
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             gpsPoints[5];           // stored position to fly to (currently only 1 target supported)
//gpsInfo_t             *gpsTarget;                     // points to one of the targets
gpsInfo_t               actualPos;                      // measured position (last gps record)
 
#define SYNC_CHAR1              0xb5
#define SYNC_CHAR2              0x62
 
#define CLASS_NAV               0x01
#define MSGID_POSECEF   0x01
#define MSGID_STATUS    0x03
//#define       MSGID_POSLLH    0x02
#define MSGID_VELECEF   0x11
//#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 {
        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;
*/
 
typedef struct {
        unsigned long   ITOW;           // ms  GPS Millisecond Time of Week  
        long                    ECEF_X;                 // ecef x / cm 
        long                    ECEF_Y;                 // ecef y / cm  
        long                    ECEF_Z;                 // ecef z / cm  
        unsigned long   Pacc;           // Abweichung  
        uint8_t                 packetStatus;
} NAV_POSECEF_t ;
 
typedef struct {
        unsigned long   ITOW;           // ms  GPS Millisecond Time of Week  
        long                    ECEFVX;                 // ecef x velocity cm/s 
        long                    ECEFVY;                 // ecef y velocity cm/s   
        long                    ECEFVZ;                 // ecef z velocity cm/s 
        unsigned long   SAcc;           // Abweichung  
        uint8_t                 packetStatus;
} NAV_VELECEF_t;
 
 
NAV_STATUS_t    navStatus;
NAV_POSECEF_t   navPosECEF;
NAV_VELECEF_t   navVelECEF;
 
//NAV_POSLLH_t  navPosLlh;
//NAV_POSUTM_t  navPosUtm;
//NAV_VELECEF  avVelNed;
 
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
 
 
// distance to target position
long                    rollOffset;                     // in 10cm
long                    nickOffset;
 
#define GPS_INTCYCLES 100
#define GPS_I_LIMIT             (long)(40 * MAINLOOPS_PER_SEC)
 
#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
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_TX)
{
 static unsigned int ptr = 0;
 unsigned char tmp_tx;
 if(!UebertragungAbgeschlossen)  
  {
   ptr++;                    // die [0] wurde schon gesendet
   tmp_tx = SendeBuffer[ptr];  
   if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
    {
     ptr = 0;
     UebertragungAbgeschlossen = 1;
    }
   UDR = tmp_tx;
  }
  else ptr = 0;
}
 
void GPSscanData (void)
{
 
 
if (navPosECEF.packetStatus == 1)                       // valid packet
        {
                actualPos.x = navPosECEF.ECEF_X;  //ECEF X in cm
                actualPos.y = navPosECEF.ECEF_Y;  //ECEF Y in cm
                actualPos.z = navPosECEF.ECEF_Z; //ECEF Z in cm
                navPosECEF.packetStatus = 0;
        }
       
 
        if (navStatus.packetStatus == 1)                        // valid packet
        {
                actualPos.state = navStatus.GPSfix;
                navStatus.packetStatus = 0;
        }
       
        if (navVelECEF.packetStatus == 1)                       // valid packet
        {
                actualPos.vx = navVelECEF.ECEFVX;  //ECEF VEL X in cm/s
                actualPos.vy = navVelECEF.ECEFVY;  //ECEF VEL Y in cm/s
                actualPos.vz = navVelECEF.ECEFVZ; //ECEF VEL Z in cm/s
                navVelECEF.packetStatus = 0;
        }
/*
        if (navPosUtm.packetStatus == 1)                        // valid packet
        {
                actualPos.northing = navPosUtm.NORTH;  ///10;           // in 10cm;
                actualPos.easting = navPosUtm.EAST;  //10;
                actualPos.altitude = navPosUtm.ALT; //10;
                navPosUtm.packetStatus = 0;
        }
 
 
        if (navPosLlh.packetStatus == 1)
                navPosLlh.packetStatus = 0;
               
        if (navVelNed.packetStatus == 1){
                actualPos.velNorth = navVelNed.VEL_N;
                actualPos.velEast = navVelNed.VEL_E;
                navVelNed.packetStatus = 0;}
 
        navPosLlh and navVelNed currently not used
*/
 
 if (actualPos.state != 0){ROT_ON;}             //-> Rot blinkt mit 4Hz wenn GPS Signal brauchbar
 
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_RX)
{
 static unsigned int crc;
 static unsigned char crc1,crc2,buf_ptr;
 static unsigned char UartState = 0;
 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_POSECEF:
                                                ubxP = (char*)&navPosECEF;
                                                ubxEp = (char*)(&navPosECEF + sizeof(NAV_POSECEF_t));
                                                ubxSp = (char*)&navPosECEF.packetStatus;
                                                ignorePacket = navPosECEF.packetStatus;
                                                break;
                                        case MSGID_VELECEF:
                                                ubxP = (char*)&navVelECEF;
                                                ubxEp = (char*)(&navVelECEF + sizeof(NAV_VELECEF_t));
                                                ubxSp = (char*)&navVelECEF.packetStatus;
                                                ignorePacket = navVelECEF.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
                                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 ();                                                 //Test kann ggf. wieder gelöscht werden!
        }
        GPSscanData ();
 
 
 
 
 
 
 
 if(buf_ptr >= MAX_EMPFANGS_BUFF)    UartState = 0;
 if(SioTmp == '\r' && UartState == 2)
  {
   UartState = 0;
   crc -= RxdBuffer[buf_ptr-2];
   crc -= RxdBuffer[buf_ptr-1];
   crc %= 4096;
   crc1 = '=' + crc / 64;
   crc2 = '=' + crc % 64;
   CrcOkay = 0;
   if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
   if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
    {
     NeuerDatensatzEmpfangen = 1;
         AnzahlEmpfangsBytes = buf_ptr;
     RxdBuffer[buf_ptr] = '\r';
         if(/*(RxdBuffer[1] == MeineSlaveAdresse || (RxdBuffer[1] == 'a')) && */(RxdBuffer[2] == 'R')) wdt_enable(WDTO_250MS); // Reset-Commando
        }                                
  }
  else
  switch(UartState)
  {
   case 0:
          if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1;  // Startzeichen und Daten schon verarbeitet
                  buf_ptr = 0;
                  RxdBuffer[buf_ptr++] = SioTmp;
                  crc = SioTmp;
          break;
   case 1: // Adresse auswerten
                  UartState++;
                  RxdBuffer[buf_ptr++] = SioTmp;
                  crc += SioTmp;
                  break;
   case 2: //  Eingangsdaten sammeln
                  RxdBuffer[buf_ptr] = SioTmp;
                  if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
                  else UartState = 0;
                  crc += SioTmp;
                  break;
   default:
          UartState = 0;
          break;
  }
}
 
 
// --------------------------------------------------------------------------
void AddCRC(unsigned int wieviele)
{
 unsigned int tmpCRC = 0,i;
 for(i = 0; i < wieviele;i++)
  {
   tmpCRC += SendeBuffer[i];
  }
   tmpCRC %= 4096;
   SendeBuffer[i++] = '=' + tmpCRC / 64;
   SendeBuffer[i++] = '=' + tmpCRC % 64;
   SendeBuffer[i++] = '\r';
  UebertragungAbgeschlossen = 0;
  UDR = SendeBuffer[0];
}
 
 
// --------------------------------------------------------------------------
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
{
 unsigned int pt = 0;
 unsigned char a,b,c;
 unsigned char ptr = 0;
 
 SendeBuffer[pt++] = '#';               // Startzeichen
 SendeBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
 SendeBuffer[pt++] = cmd;                       // Commando
 
 while(len)
  {
   if(len) { a = snd[ptr++]; len--;} else a = 0;
   if(len) { b = snd[ptr++]; len--;} else b = 0;
   if(len) { c = snd[ptr++]; len--;} else c = 0;
   SendeBuffer[pt++] = '=' + (a >> 2);
   SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
   SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
   SendeBuffer[pt++] = '=' + ( c & 0x3f);
  }
 AddCRC(pt);
}
 
 
// --------------------------------------------------------------------------
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max)  // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
{
 unsigned char a,b,c,d;
 unsigned char ptr = 0;
 unsigned char x,y,z;
 while(len)
  {
   a = RxdBuffer[ptrIn++] - '=';
   b = RxdBuffer[ptrIn++] - '=';
   c = RxdBuffer[ptrIn++] - '=';
   d = RxdBuffer[ptrIn++] - '=';
   if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden
 
   x = (a << 2) | (b >> 4);
   y = ((b & 0x0f) << 4) | (c >> 2);
   z = ((c & 0x03) << 6) | d;
 
   if(len--) ptrOut[ptr++] = x; else break;
   if(len--) ptrOut[ptr++] = y; else break;
   if(len--) ptrOut[ptr++] = z; else break;
  }
 
}
 
// --------------------------------------------------------------------------
void BearbeiteRxDaten(void)
{
 if(!NeuerDatensatzEmpfangen) return;
 
 unsigned int tmp_int_arr1[1];
 unsigned int tmp_int_arr2[2];
 unsigned int tmp_int_arr3[3];
 unsigned char tmp_char_arr2[2];
 unsigned char tmp_char_arr3[3];
 unsigned char tmp_char_arr4[4];
 //if(!MotorenEin) 
 PcZugriff = 255;
  switch(RxdBuffer[2])
  {
   case 'c':// Debugdaten incl. Externe IOs usw 
                        Decode64((unsigned char *) &DebugIn,sizeof(DebugIn),3,AnzahlEmpfangsBytes);
/*              for(unsigned char i=0; i<4;i++)
                         {
              EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2],     DebugIn.Analog[i]);
                          EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2 + 1], DebugIn.Analog[i] >> 8);       
                         }*/
                        RemoteTasten |= DebugIn.RemoteTasten;
            DebugDataAnforderung = 1;
            break;
 
   case 'h':// x-1 Displayzeilen
            Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
            RemoteTasten |= tmp_char_arr2[0];
                        DebugDisplayAnforderung = 1;
                        break;
   case 't':// Motortest
            Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
                        break;
   case 'v': // Version-Anforderung     und Ausbaustufe
            GetVersionAnforderung = 1;
            break;                                                               
   case 'g':// "Get"-Anforderung für Debug-Daten 
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
            DebugGetAnforderung = 1;
            break;
   case 'q':// "Get"-Anforderung für Settings
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
            Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
            if(tmp_char_arr2[0] != 0xff)
             {
                          if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;
                  ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);                   
                  SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
             }
             else
                  SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
             
            break;
       
   case 'l':
   case 'm':
   case 'n':
   case 'o':
   case 'p': // Parametersatz speichern
            Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes);
                        WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
            eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1);  // aktiven Datensatz merken
            Piep(GetActiveParamSetNumber());
         break;
               
         
  }
// DebugOut.AnzahlZyklen =  Debug_Timer_Intervall;
 NeuerDatensatzEmpfangen = 0;
}
 
//############################################################################
//Routine für die Serielle Ausgabe
int uart_putchar (char c)
//############################################################################
{
        if (c == '\n')
                uart_putchar('\r');
        //Warten solange bis Zeichen gesendet wurde
        loop_until_bit_is_set(USR, UDRE);
        //Ausgabe des Zeichens
        UDR = c;
       
        return (0);
}
 
// --------------------------------------------------------------------------
void WriteProgramData(unsigned int pos, unsigned char wert)
{
  //if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
  // else eeprom_write_byte(&EE_Buffer[pos], wert);
  // Buffer[pos] = wert;
}
 
//############################################################################
//INstallation der Seriellen Schnittstelle
void UART_Init (void)
//############################################################################
{
        //Enable TXEN im Register UCR TX-Data Enable & RX Enable
 
        UCR=(1 << TXEN) | (1 << RXEN);
    // UART Double Speed (U2X)
        USR   |= (1<<U2X);          
        // RX-Interrupt Freigabe
        UCSRB |= (1<<RXCIE);          
        // TX-Interrupt Freigabe
        UCSRB |= (1<<TXCIE);          
 
        //Teiler wird gesetzt 
        UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);
        //UBRR = 33;
        //öffnet einen Kanal für printf (STDOUT)
        //fdevopen (uart_putchar, 0);
        //sbi(PORTD,4);
  Debug_Timer = SetDelay(200);  
 
  gpsState = GPS_EMPTY;
}
 
//---------------------------------------------------------------------------------------------
void DatenUebertragung(void)  
{
 static char dis_zeile = 0;
 if(!UebertragungAbgeschlossen) return;
 
   if(DebugGetAnforderung && UebertragungAbgeschlossen)               // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
   {
      SendOutData('G',MeineSlaveAdresse,(unsigned char *) &DebugIn,sizeof(DebugIn));
          DebugGetAnforderung = 0;
   }
 
    if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)  
         {
          SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
          DebugDataAnforderung = 0;
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);  
         }
 
     if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
         {
      Menu();
          DebugDisplayAnforderung = 0;
      if(++dis_zeile == 4) dis_zeile = 0;
      SendOutData('0' + dis_zeile,0,&DisplayBuff[20 * dis_zeile],20);   // DisplayZeile übertragen
         }
    if(GetVersionAnforderung && UebertragungAbgeschlossen)
     {
      SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo));
          GetVersionAnforderung = 0;
     }
 
}
 
 

Rev 36	Rev 37
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + only for non-profit use	3	// + only for non-profit use
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// + see the File "License.txt" for further Informations	5	// + see the File "License.txt" for further Informations
6	// +	6	// +
7	// + GPS read out:	7	// + GPS read out:
8	// + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany	8	// + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany
9	// + only for non-profit use	9	// + only for non-profit use
10	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	10	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11		11
12	#include "main.h"	12	#include "main.h"
13	#include "uart.h"	13	#include "uart.h"
14		14
15	unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;	15	unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
16	unsigned volatile char SioTmp = 0;	16	unsigned volatile char SioTmp = 0;
17	unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];	17	unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
18	unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];	18	unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
19	unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];	19	unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
20	unsigned volatile char NeuerDatensatzEmpfangen = 0;	20	unsigned volatile char NeuerDatensatzEmpfangen = 0;
21	unsigned volatile char NeueKoordinateEmpfangen = 0;	21	unsigned volatile char NeueKoordinateEmpfangen = 0;
22	unsigned volatile char UebertragungAbgeschlossen = 1;	22	unsigned volatile char UebertragungAbgeschlossen = 1;
23	unsigned volatile char CntCrcError = 0;	23	unsigned volatile char CntCrcError = 0;
24	unsigned volatile char AnzahlEmpfangsBytes = 0;	24	unsigned volatile char AnzahlEmpfangsBytes = 0;
25	unsigned volatile char PC_DebugTimeout = 0;	25	unsigned volatile char PC_DebugTimeout = 0;
26	unsigned char PcZugriff = 100;	26	unsigned char PcZugriff = 100;
27	unsigned char MotorTest[4] = {0,0,0,0};	27	unsigned char MotorTest[4] = {0,0,0,0};
28	unsigned char MeineSlaveAdresse;	28	unsigned char MeineSlaveAdresse;
29	struct str_DebugOut DebugOut;	29	struct str_DebugOut DebugOut;
30	struct str_Debug DebugIn;	30	struct str_Debug DebugIn;
31	struct str_VersionInfo VersionInfo;	31	struct str_VersionInfo VersionInfo;
32	int Debug_Timer;	32	int Debug_Timer;
33		33
34	static uint8_t gpsState;	34	static uint8_t gpsState;
35	#define GPS_EMPTY 0	35	#define GPS_EMPTY 0
36	#define GPS_SYNC1 1	36	#define GPS_SYNC1 1
37	#define GPS_SYNC2 2	37	#define GPS_SYNC2 2
38	#define GPS_CLASS 3	38	#define GPS_CLASS 3
39	#define GPS_LEN1 4	39	#define GPS_LEN1 4
40	#define GPS_LEN2 5	40	#define GPS_LEN2 5
41	#define GPS_FILLING 6	41	#define GPS_FILLING 6
42	#define GPS_CKA 7	42	#define GPS_CKA 7
43	#define GPS_CKB 8	43	#define GPS_CKB 8
44		44
45	//gpsInfo_t gpsPoints[5]; // stored position to fly to (currently only 1 target supported)	45	//gpsInfo_t gpsPoints[5]; // stored position to fly to (currently only 1 target supported)
46	//gpsInfo_t *gpsTarget; // points to one of the targets	46	//gpsInfo_t *gpsTarget; // points to one of the targets
47	gpsInfo_t actualPos; // measured position (last gps record)	47	gpsInfo_t actualPos; // measured position (last gps record)
48		48
49	#define SYNC_CHAR1 0xb5	49	#define SYNC_CHAR1 0xb5
50	#define SYNC_CHAR2 0x62	50	#define SYNC_CHAR2 0x62
51		51
52	#define CLASS_NAV 0x01	52	#define CLASS_NAV 0x01
-		53	#define MSGID_POSECEF 0x01
53	#define MSGID_STATUS 0x03	54	#define MSGID_STATUS 0x03
54	#define MSGID_POSLLH 0x02	55	//#define MSGID_POSLLH 0x02
-		56	#define MSGID_VELECEF 0x11
55	#define MSGID_POSUTM 0x08	57	//#define MSGID_POSUTM 0x08
56	#define MSGID_VELNED 0x12	58	//#define MSGID_VELNED 0x12
57		59
58		60
59		61
60	typedef struct {	62	typedef struct {
61	unsigned long ITOW; // time of week	63	unsigned long ITOW; // time of week
62	uint8_t GPSfix; // GPSfix Type, range 0..6	64	uint8_t GPSfix; // GPSfix Type, range 0..6
63	uint8_t Flags; // Navigation Status Flags	65	uint8_t Flags; // Navigation Status Flags
64	uint8_t DiffS; // Differential Status	66	uint8_t DiffS; // Differential Status
65	uint8_t res; // reserved	67	uint8_t res; // reserved
66	unsigned long TTFF; // Time to first fix (millisecond time tag)	68	unsigned long TTFF; // Time to first fix (millisecond time tag)
67	unsigned long MSSS; // Milliseconds since Startup / Reset	69	unsigned long MSSS; // Milliseconds since Startup / Reset
68	uint8_t packetStatus;	70	uint8_t packetStatus;
69	} NAV_STATUS_t;	71	} NAV_STATUS_t;
70		72
71		73	/*
72	typedef struct {	74	typedef struct {
73	unsigned long ITOW; // time of week	75	unsigned long ITOW; // time of week
74	long LON; // longitude in 1e-07 deg	76	long LON; // longitude in 1e-07 deg
75	long LAT; // lattitude	77	long LAT; // lattitude
76	long HEIGHT; // height in mm	78	long HEIGHT; // height in mm
77	long HMSL; // height above mean sea level im mm	79	long HMSL; // height above mean sea level im mm
78	unsigned long Hacc; // horizontal accuracy in mm	80	unsigned long Hacc; // horizontal accuracy in mm
79	unsigned long Vacc; // vertical accuracy in mm	81	unsigned long Vacc; // vertical accuracy in mm
80	uint8_t packetStatus;	82	uint8_t packetStatus;
81	} NAV_POSLLH_t;	83	} NAV_POSLLH_t;
82		84
83		85
84	typedef struct {	86	typedef struct {
85	unsigned long ITOW; // time of week	87	unsigned long ITOW; // time of week
86	long EAST; // cm UTM Easting	88	long EAST; // cm UTM Easting
87	long NORTH; // cm UTM Nording	89	long NORTH; // cm UTM Nording
88	long ALT; // cm altitude	90	long ALT; // cm altitude
89	uint8_t ZONE; // UTM zone number	91	uint8_t ZONE; // UTM zone number
90	uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)	92	uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
91	uint8_t packetStatus;	93	uint8_t packetStatus;
92	} NAV_POSUTM_t;	94	} NAV_POSUTM_t;
93		95
94		96
95	typedef struct {	97	typedef struct {
96	unsigned long ITOW; // ms GPS Millisecond Time of Week	98	unsigned long ITOW; // ms GPS Millisecond Time of Week
97	long VEL_N; // cm/s NED north velocity	99	long VEL_N; // cm/s NED north velocity
98	long VEL_E; // cm/s NED east velocity	100	long VEL_E; // cm/s NED east velocity
99	long VEL_D; // cm/s NED down velocity	101	long VEL_D; // cm/s NED down velocity
100	unsigned long Speed; // cm/s Speed (3-D)	102	unsigned long Speed; // cm/s Speed (3-D)
101	unsigned long GSpeed; // cm/s Ground Speed (2-D)	103	unsigned long GSpeed; // cm/s Ground Speed (2-D)
102	long Heading; // deg (1e-05) Heading 2-D	104	long Heading; // deg (1e-05) Heading 2-D
103	unsigned long SAcc; // cm/s Speed Accuracy Estimate	105	unsigned long SAcc; // cm/s Speed Accuracy Estimate
104	unsigned long CAcc; // deg Course / Heading Accuracy Estimate	106	unsigned long CAcc; // deg Course / Heading Accuracy Estimate
105	uint8_t packetStatus;	107	uint8_t packetStatus;
106	} NAV_VELNED_t;	108	} NAV_VELNED_t;
-		109	*/
-		110
-		111	typedef struct {
-		112	unsigned long ITOW; // ms GPS Millisecond Time of Week
-		113	long ECEF_X; // ecef x / cm
-		114	long ECEF_Y; // ecef y / cm
-		115	long ECEF_Z; // ecef z / cm
-		116	unsigned long Pacc; // Abweichung
-		117	uint8_t packetStatus;
-		118	} NAV_POSECEF_t ;
-		119
-		120	typedef struct {
-		121	unsigned long ITOW; // ms GPS Millisecond Time of Week
-		122	long ECEFVX; // ecef x velocity cm/s
-		123	long ECEFVY; // ecef y velocity cm/s
-		124	long ECEFVZ; // ecef z velocity cm/s
-		125	unsigned long SAcc; // Abweichung
-		126	uint8_t packetStatus;
-		127	} NAV_VELECEF_t;
107		128
108		129
109	NAV_STATUS_t navStatus;	130	NAV_STATUS_t navStatus;
-		131	NAV_POSECEF_t navPosECEF;
-		132	NAV_VELECEF_t navVelECEF;
-		133
110	NAV_POSLLH_t navPosLlh;	134	//NAV_POSLLH_t navPosLlh;
111	NAV_POSUTM_t navPosUtm;	135	//NAV_POSUTM_t navPosUtm;
112	NAV_VELNED_t navVelNed;	136	//NAV_VELECEF avVelNed;
113		137
114	volatile char ubxP, ubxEp, *ubxSp; // pointers to packet currently transfered	138	volatile char ubxP, ubxEp, *ubxSp; // pointers to packet currently transfered
115	volatile uint8_t CK_A, CK_B; // UBX checksum bytes	139	volatile uint8_t CK_A, CK_B; // UBX checksum bytes
116	volatile unsigned short msgLen;	140	volatile unsigned short msgLen;
117	volatile uint8_t msgID;	141	volatile uint8_t msgID;
118	volatile uint8_t ignorePacket; // true when previous packet was not processed	142	volatile uint8_t ignorePacket; // true when previous packet was not processed
119		143
120		144
121	// distance to target position	145	// distance to target position
122	long rollOffset; // in 10cm	146	long rollOffset; // in 10cm
123	long nickOffset;	147	long nickOffset;
124		148
125	#define GPS_INTCYCLES 100	149	#define GPS_INTCYCLES 100
126	#define GPS_I_LIMIT (long)(40 * MAINLOOPS_PER_SEC)	150	#define GPS_I_LIMIT (long)(40 * MAINLOOPS_PER_SEC)
127		151
128	#ifdef GPS_DEBUG // if set then the GPS data is transfered to display	152	#ifdef GPS_DEBUG // if set then the GPS data is transfered to display
129	extern volatile uint8_t v24state;	153	extern volatile uint8_t v24state;
130	char buf[200];	154	char buf[200];
131	char *bp;	155	char *bp;
132	char *ep;	156	char *ep;
133	#endif	157	#endif
134		158
135	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	159	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
136	//++ Sende-Part der Datenübertragung	160	//++ Sende-Part der Datenübertragung
137	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	161	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
138	SIGNAL(INT_VEC_TX)	162	SIGNAL(INT_VEC_TX)
139	{	163	{
140	static unsigned int ptr = 0;	164	static unsigned int ptr = 0;
141	unsigned char tmp_tx;	165	unsigned char tmp_tx;
142	if(!UebertragungAbgeschlossen)	166	if(!UebertragungAbgeschlossen)
143	{	167	{
144	ptr++; // die [0] wurde schon gesendet	168	ptr++; // die [0] wurde schon gesendet
145	tmp_tx = SendeBuffer[ptr];	169	tmp_tx = SendeBuffer[ptr];
146	if((tmp_tx == '\r') \|\| (ptr == MAX_SENDE_BUFF))	170	if((tmp_tx == '\r') \|\| (ptr == MAX_SENDE_BUFF))
147	{	171	{
148	ptr = 0;	172	ptr = 0;
149	UebertragungAbgeschlossen = 1;	173	UebertragungAbgeschlossen = 1;
150	}	174	}
151	UDR = tmp_tx;	175	UDR = tmp_tx;
152	}	176	}
153	else ptr = 0;	177	else ptr = 0;
154	}	178	}
155		179
156	void GPSscanData (void)	180	void GPSscanData (void)
157	{	181	{
-		182
-		183
-		184	if (navPosECEF.packetStatus == 1) // valid packet
-		185	{
-		186	actualPos.x = navPosECEF.ECEF_X; //ECEF X in cm
-		187	actualPos.y = navPosECEF.ECEF_Y; //ECEF Y in cm
-		188	actualPos.z = navPosECEF.ECEF_Z; //ECEF Z in cm
-		189	navPosECEF.packetStatus = 0;
-		190	}
-		191
-		192
158	if (navStatus.packetStatus == 1) // valid packet	193	if (navStatus.packetStatus == 1) // valid packet
159	{	194	{
160	actualPos.state = navStatus.GPSfix;	195	actualPos.state = navStatus.GPSfix;
161	navStatus.packetStatus = 0;	196	navStatus.packetStatus = 0;
162	}	197	}
163		198
-		199	if (navVelECEF.packetStatus == 1) // valid packet
-		200	{
-		201	actualPos.vx = navVelECEF.ECEFVX; //ECEF VEL X in cm/s
-		202	actualPos.vy = navVelECEF.ECEFVY; //ECEF VEL Y in cm/s
-		203	actualPos.vz = navVelECEF.ECEFVZ; //ECEF VEL Z in cm/s
-		204	navVelECEF.packetStatus = 0;
-		205	}
-		206	/*
164	if (navPosUtm.packetStatus == 1) // valid packet	207	if (navPosUtm.packetStatus == 1) // valid packet
165	{	208	{
166	actualPos.northing = navPosUtm.NORTH; ///10; // in 10cm;	209	actualPos.northing = navPosUtm.NORTH; ///10; // in 10cm;
167	actualPos.easting = navPosUtm.EAST; //10;	210	actualPos.easting = navPosUtm.EAST; //10;
168	actualPos.altitude = navPosUtm.ALT; //10;	211	actualPos.altitude = navPosUtm.ALT; //10;
169	navPosUtm.packetStatus = 0;	212	navPosUtm.packetStatus = 0;
170	}	213	}
171		214
172		215
173	if (navPosLlh.packetStatus == 1)	216	if (navPosLlh.packetStatus == 1)
174	navPosLlh.packetStatus = 0;	217	navPosLlh.packetStatus = 0;
175		218
176	if (navVelNed.packetStatus == 1){	219	if (navVelNed.packetStatus == 1){
177	actualPos.velNorth = navVelNed.VEL_N;	220	actualPos.velNorth = navVelNed.VEL_N;
178	actualPos.velEast = navVelNed.VEL_E;	221	actualPos.velEast = navVelNed.VEL_E;
179	navVelNed.packetStatus = 0;}	222	navVelNed.packetStatus = 0;}
180	/*	223
181	navPosLlh and navVelNed currently not used	224	navPosLlh and navVelNed currently not used
182	*/	225	*/
-		226
183	if (actualPos.state != 0){ROT_ON;} //-> Rot blinkt mit 4Hz wenn GPS Signal brauchbar	227	if (actualPos.state != 0){ROT_ON;} //-> Rot blinkt mit 4Hz wenn GPS Signal brauchbar
184		228
185	}	229	}
186		230
187	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	231	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
188	//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung	232	//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
189	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	233	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
190	SIGNAL(INT_VEC_RX)	234	SIGNAL(INT_VEC_RX)
191	{	235	{
192	static unsigned int crc;	236	static unsigned int crc;
193	static unsigned char crc1,crc2,buf_ptr;	237	static unsigned char crc1,crc2,buf_ptr;
194	static unsigned char UartState = 0;	238	static unsigned char UartState = 0;
195	unsigned char CrcOkay = 0;	239	unsigned char CrcOkay = 0;
196		240
197	SioTmp = UDR;	241	SioTmp = UDR;
198		242
199		243
200		244
201		245
202	uint8_t c;	246	uint8_t c;
203	uint8_t re;	247	uint8_t re;
204		248
205	re = (UCSR0A & (_B1(FE0) \| _B1(DOR0))); // any error occured ?	249	re = (UCSR0A & (_B1(FE0) \| _B1(DOR0))); // any error occured ?
206	c = SioTmp;	250	c = SioTmp;
207		251
208	#ifdef GPS_DEBUG	252	#ifdef GPS_DEBUG
209	*bp++ = c;	253	*bp++ = c;
210	if (bp >= (buf+200)) bp = buf;	254	if (bp >= (buf+200)) bp = buf;
211	if (v24state == 0)	255	if (v24state == 0)
212	{	256	{
213	v24state = 1;	257	v24state = 1;
214	UDR0 = *ep++;	258	UDR0 = *ep++;
215	if (ep >= buf+200)	259	if (ep >= buf+200)
216	ep = buf;	260	ep = buf;
217	UCSR0B \|= _B1(UDRIE0); //enable further irqs	261	UCSR0B \|= _B1(UDRIE0); //enable further irqs
218	}	262	}
219	#endif	263	#endif
220		264
221	if (re == 0)	265	if (re == 0)
222	{	266	{
223	switch (gpsState)	267	switch (gpsState)
224	{	268	{
225	case GPS_EMPTY:	269	case GPS_EMPTY:
226	if (c == SYNC_CHAR1)	270	if (c == SYNC_CHAR1)
227	gpsState = GPS_SYNC1;	271	gpsState = GPS_SYNC1;
228	break;	272	break;
229	case GPS_SYNC1:	273	case GPS_SYNC1:
230	if (c == SYNC_CHAR2)	274	if (c == SYNC_CHAR2)
231	gpsState = GPS_SYNC2;	275	gpsState = GPS_SYNC2;
232	else if (c != SYNC_CHAR1)	276	else if (c != SYNC_CHAR1)
233	gpsState = GPS_EMPTY;	277	gpsState = GPS_EMPTY;
234	break;	278	break;
235	case GPS_SYNC2:	279	case GPS_SYNC2:
236	if (c == CLASS_NAV)	280	if (c == CLASS_NAV)
237	gpsState = GPS_CLASS;	281	gpsState = GPS_CLASS;
238	else	282	else
239	gpsState = GPS_EMPTY;	283	gpsState = GPS_EMPTY;
240	break;	284	break;
241	case GPS_CLASS: // msg ID seen: init packed receive	285	case GPS_CLASS: // msg ID seen: init packed receive
242	msgID = c;	286	msgID = c;
243	CK_A = CLASS_NAV + c;	287	CK_A = CLASS_NAV + c;
244	CK_B = CLASS_NAV + CK_A;	288	CK_B = CLASS_NAV + CK_A;
245	gpsState = GPS_LEN1;	289	gpsState = GPS_LEN1;
246		290
247	switch (msgID)	291	switch (msgID)
248	{	292	{
249	case MSGID_STATUS:	293	case MSGID_STATUS:
250	ubxP = (char*)&navStatus;	294	ubxP = (char*)&navStatus;
251	ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));	295	ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
252	ubxSp = (char*)&navStatus.packetStatus;	296	ubxSp = (char*)&navStatus.packetStatus;
253	ignorePacket = navStatus.packetStatus;	297	ignorePacket = navStatus.packetStatus;
254	break;	298	break;
-		299	case MSGID_POSECEF:
-		300	ubxP = (char*)&navPosECEF;
-		301	ubxEp = (char*)(&navPosECEF + sizeof(NAV_POSECEF_t));
-		302	ubxSp = (char*)&navPosECEF.packetStatus;
-		303	ignorePacket = navPosECEF.packetStatus;
-		304	break;
-		305	case MSGID_VELECEF:
-		306	ubxP = (char*)&navVelECEF;
-		307	ubxEp = (char*)(&navVelECEF + sizeof(NAV_VELECEF_t));
-		308	ubxSp = (char*)&navVelECEF.packetStatus;
-		309	ignorePacket = navVelECEF.packetStatus;
-		310	break;
-		311
-		312	/*
255	case MSGID_POSLLH:	313	case MSGID_POSLLH:
256	ubxP = (char*)&navPosLlh;	314	ubxP = (char*)&navPosLlh;
257	ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));	315	ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
258	ubxSp = (char*)&navPosLlh.packetStatus;	316	ubxSp = (char*)&navPosLlh.packetStatus;
259	ignorePacket = navPosLlh.packetStatus;	317	ignorePacket = navPosLlh.packetStatus;
260	break;	318	break;
261	case MSGID_POSUTM:	319	case MSGID_POSUTM:
262	ubxP = (char*)&navPosUtm;	320	ubxP = (char*)&navPosUtm;
263	ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));	321	ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
264	ubxSp = (char*)&navPosUtm.packetStatus;	322	ubxSp = (char*)&navPosUtm.packetStatus;
265	ignorePacket = navPosUtm.packetStatus;	323	ignorePacket = navPosUtm.packetStatus;
266	break;	324	break;
267	case MSGID_VELNED:	325	case MSGID_VELNED:
268	ubxP = (char*)&navVelNed;	326	ubxP = (char*)&navVelNed;
269	ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));	327	ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
270	ubxSp = (char*)&navVelNed.packetStatus;	328	ubxSp = (char*)&navVelNed.packetStatus;
271	ignorePacket = navVelNed.packetStatus;	329	ignorePacket = navVelNed.packetStatus;
272	break;	330	break;
-		331	*/
-		332
273	default:	333	default:
274	ignorePacket = 1;	334	ignorePacket = 1;
275	ubxSp = (char*)0;	335	ubxSp = (char*)0;
276	}	336	}
277	break;	337	break;
278	case GPS_LEN1: // first len byte	338	case GPS_LEN1: // first len byte
279	msgLen = c;	339	msgLen = c;
280	CK_A += c;	340	CK_A += c;
281	CK_B += CK_A;	341	CK_B += CK_A;
282	gpsState = GPS_LEN2;	342	gpsState = GPS_LEN2;
283	break;	343	break;
284	case GPS_LEN2: // second len byte	344	case GPS_LEN2: // second len byte
285	msgLen = msgLen + (c * 256);	345	msgLen = msgLen + (c * 256);
286	CK_A += c;	346	CK_A += c;
287	CK_B += CK_A;	347	CK_B += CK_A;
288	gpsState = GPS_FILLING; // next data will be stored in packet struct	348	gpsState = GPS_FILLING; // next data will be stored in packet struct
289	break;	349	break;
290	case GPS_FILLING:	350	case GPS_FILLING:
291	CK_A += c;	351	CK_A += c;
292	CK_B += CK_A;	352	CK_B += CK_A;
293		353
294	if ( !ignorePacket && ubxP < ubxEp)	354	if ( !ignorePacket && ubxP < ubxEp)
295	*ubxP++ = c;	355	*ubxP++ = c;
296		356
297	if (--msgLen == 0)	357	if (--msgLen == 0)
298	gpsState = GPS_CKA;	358	gpsState = GPS_CKA;
299		359
300	break;	360	break;
301	case GPS_CKA:	361	case GPS_CKA:
302	if (c == CK_A)	362	if (c == CK_A)
303	gpsState = GPS_CKB;	363	gpsState = GPS_CKB;
304	else	364	else
305	gpsState = GPS_EMPTY;	365	gpsState = GPS_EMPTY;
306	break;	366	break;
307	case GPS_CKB:	367	case GPS_CKB:
308	if (c == CK_B && ubxSp) // No error -> packet received successfully	368	if (c == CK_B && ubxSp) // No error -> packet received successfully
309	*ubxSp = 1; // set packetStatus in struct	369	*ubxSp = 1; // set packetStatus in struct
310	gpsState = GPS_EMPTY; // ready for next packet	370	gpsState = GPS_EMPTY; // ready for next packet
311	break;	371	break;
312	default:	372	default:
313	gpsState = GPS_EMPTY; // ready for next packet	373	gpsState = GPS_EMPTY; // ready for next packet
314	}	374	}
315	}	375	}
316	else // discard any data if error occured	376	else // discard any data if error occured
317	{	377	{
318	gpsState = GPS_EMPTY;	378	gpsState = GPS_EMPTY;
319	GPSscanData (); //Test kann ggf. wieder gelöscht werden!	379	GPSscanData (); //Test kann ggf. wieder gelöscht werden!
320	}	380	}
321	GPSscanData ();	381	GPSscanData ();
322		382
323		383
324		384
325		385
326		386
327		387
328		388
329	if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;	389	if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;
330	if(SioTmp == '\r' && UartState == 2)	390	if(SioTmp == '\r' && UartState == 2)
331	{	391	{
332	UartState = 0;	392	UartState = 0;
333	crc -= RxdBuffer[buf_ptr-2];	393	crc -= RxdBuffer[buf_ptr-2];
334	crc -= RxdBuffer[buf_ptr-1];	394	crc -= RxdBuffer[buf_ptr-1];
335	crc %= 4096;	395	crc %= 4096;
336	crc1 = '=' + crc / 64;	396	crc1 = '=' + crc / 64;
337	crc2 = '=' + crc % 64;	397	crc2 = '=' + crc % 64;
338	CrcOkay = 0;	398	CrcOkay = 0;
339	if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};	399	if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
340	if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet	400	if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
341	{	401	{
342	NeuerDatensatzEmpfangen = 1;	402	NeuerDatensatzEmpfangen = 1;
343	AnzahlEmpfangsBytes = buf_ptr;	403	AnzahlEmpfangsBytes = buf_ptr;
344	RxdBuffer[buf_ptr] = '\r';	404	RxdBuffer[buf_ptr] = '\r';
345	if(/(RxdBuffer[1] == MeineSlaveAdresse \|\| (RxdBuffer[1] == 'a')) && /(RxdBuffer[2] == 'R')) wdt_enable(WDTO_250MS); // Reset-Commando	405	if(/(RxdBuffer[1] == MeineSlaveAdresse \|\| (RxdBuffer[1] == 'a')) && /(RxdBuffer[2] == 'R')) wdt_enable(WDTO_250MS); // Reset-Commando
346	}	406	}
347	}	407	}
348	else	408	else
349	switch(UartState)	409	switch(UartState)
350	{	410	{
351	case 0:	411	case 0:
352	if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet	412	if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet
353	buf_ptr = 0;	413	buf_ptr = 0;
354	RxdBuffer[buf_ptr++] = SioTmp;	414	RxdBuffer[buf_ptr++] = SioTmp;
355	crc = SioTmp;	415	crc = SioTmp;
356	break;	416	break;
357	case 1: // Adresse auswerten	417	case 1: // Adresse auswerten
358	UartState++;	418	UartState++;
359	RxdBuffer[buf_ptr++] = SioTmp;	419	RxdBuffer[buf_ptr++] = SioTmp;
360	crc += SioTmp;	420	crc += SioTmp;
361	break;	421	break;
362	case 2: // Eingangsdaten sammeln	422	case 2: // Eingangsdaten sammeln
363	RxdBuffer[buf_ptr] = SioTmp;	423	RxdBuffer[buf_ptr] = SioTmp;
364	if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;	424	if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
365	else UartState = 0;	425	else UartState = 0;
366	crc += SioTmp;	426	crc += SioTmp;
367	break;	427	break;
368	default:	428	default:
369	UartState = 0;	429	UartState = 0;
370	break;	430	break;
371	}	431	}
372	}	432	}
373		433
374		434
375	// --------------------------------------------------------------------------	435	// --------------------------------------------------------------------------
376	void AddCRC(unsigned int wieviele)	436	void AddCRC(unsigned int wieviele)
377	{	437	{
378	unsigned int tmpCRC = 0,i;	438	unsigned int tmpCRC = 0,i;
379	for(i = 0; i < wieviele;i++)	439	for(i = 0; i < wieviele;i++)
380	{	440	{
381	tmpCRC += SendeBuffer[i];	441	tmpCRC += SendeBuffer[i];
382	}	442	}
383	tmpCRC %= 4096;	443	tmpCRC %= 4096;
384	SendeBuffer[i++] = '=' + tmpCRC / 64;	444	SendeBuffer[i++] = '=' + tmpCRC / 64;
385	SendeBuffer[i++] = '=' + tmpCRC % 64;	445	SendeBuffer[i++] = '=' + tmpCRC % 64;
386	SendeBuffer[i++] = '\r';	446	SendeBuffer[i++] = '\r';
387	UebertragungAbgeschlossen = 0;	447	UebertragungAbgeschlossen = 0;
388	UDR = SendeBuffer[0];	448	UDR = SendeBuffer[0];
389	}	449	}
390		450
391		451
392	// --------------------------------------------------------------------------	452	// --------------------------------------------------------------------------
393	void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)	453	void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
394	{	454	{
395	unsigned int pt = 0;	455	unsigned int pt = 0;
396	unsigned char a,b,c;	456	unsigned char a,b,c;
397	unsigned char ptr = 0;	457	unsigned char ptr = 0;
398		458
399	SendeBuffer[pt++] = '#'; // Startzeichen	459	SendeBuffer[pt++] = '#'; // Startzeichen
400	SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...)	460	SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...)
401	SendeBuffer[pt++] = cmd; // Commando	461	SendeBuffer[pt++] = cmd; // Commando
402		462
403	while(len)	463	while(len)
404	{	464	{
405	if(len) { a = snd[ptr++]; len--;} else a = 0;	465	if(len) { a = snd[ptr++]; len--;} else a = 0;
406	if(len) { b = snd[ptr++]; len--;} else b = 0;	466	if(len) { b = snd[ptr++]; len--;} else b = 0;
407	if(len) { c = snd[ptr++]; len--;} else c = 0;	467	if(len) { c = snd[ptr++]; len--;} else c = 0;
408	SendeBuffer[pt++] = '=' + (a >> 2);	468	SendeBuffer[pt++] = '=' + (a >> 2);
409	SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	469	SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
410	SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	470	SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
411	SendeBuffer[pt++] = '=' + ( c & 0x3f);	471	SendeBuffer[pt++] = '=' + ( c & 0x3f);
412	}	472	}
413	AddCRC(pt);	473	AddCRC(pt);
414	}	474	}
415		475
416		476
417	// --------------------------------------------------------------------------	477	// --------------------------------------------------------------------------
418	void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer	478	void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
419	{	479	{
420	unsigned char a,b,c,d;	480	unsigned char a,b,c,d;
421	unsigned char ptr = 0;	481	unsigned char ptr = 0;
422	unsigned char x,y,z;	482	unsigned char x,y,z;
423	while(len)	483	while(len)
424	{	484	{
425	a = RxdBuffer[ptrIn++] - '=';	485	a = RxdBuffer[ptrIn++] - '=';
426	b = RxdBuffer[ptrIn++] - '=';	486	b = RxdBuffer[ptrIn++] - '=';
427	c = RxdBuffer[ptrIn++] - '=';	487	c = RxdBuffer[ptrIn++] - '=';
428	d = RxdBuffer[ptrIn++] - '=';	488	d = RxdBuffer[ptrIn++] - '=';
429	if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden	489	if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
430		490
431	x = (a << 2) \| (b >> 4);	491	x = (a << 2) \| (b >> 4);
432	y = ((b & 0x0f) << 4) \| (c >> 2);	492	y = ((b & 0x0f) << 4) \| (c >> 2);
433	z = ((c & 0x03) << 6) \| d;	493	z = ((c & 0x03) << 6) \| d;
434		494
435	if(len--) ptrOut[ptr++] = x; else break;	495	if(len--) ptrOut[ptr++] = x; else break;
436	if(len--) ptrOut[ptr++] = y; else break;	496	if(len--) ptrOut[ptr++] = y; else break;
437	if(len--) ptrOut[ptr++] = z; else break;	497	if(len--) ptrOut[ptr++] = z; else break;
438	}	498	}
439		499
440	}	500	}

Subversion Repositories FlightCtrl

(root)/branches/GPS_BETA_chris2798_hallo2/uart.c @ 849 - Rev 36 → 37