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

Subversion Repositories FlightCtrl

(root)/branches/GPS_BETA_chris2798_hallo2/uart.c @ 999 - Rev 21 → 36