WebSVN - FlightCtrl - Diff - Rev 1232 and 1253 - /branches/V0.84a_var


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdarg.h>
#include <string.h>
#include "main.h"
#include "uart.h"
 
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
unsigned char GetExternalControl = 0,DebugDisplayAnforderung1 = 0, DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0, GetPPMChannelAnforderung = 0;
unsigned char DisplayLine = 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 char *pRxData = 0;
unsigned char RxDataLen = 0;
unsigned volatile char PC_DebugTimeout = 0;
unsigned volatile char PC_MotortestActive = 0;
 
unsigned char DebugTextAnforderung = 255;
unsigned char PcZugriff = 100;
unsigned char MotorTest[16];
unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl
unsigned char ConfirmFrame;
struct str_DebugOut    DebugOut;
struct str_ExternControl  ExternControl;
struct str_VersionInfo VersionInfo;
struct str_WinkelOut WinkelOut;
struct str_Data3D Data3D;
 
int Debug_Timer,Kompass_Timer,Timer3D;
unsigned int DebugDataIntervall = 200, Intervall3D = 0;
 
const unsigned char ANALOG_TEXT[32][16] =
{
   //1234567890123456
    "AngleNick       ", //0
    "AngleRoll       ",
    "AccNick         ",
    "AccRoll         ",
    "GyroGier        ",
    "Hight Value     ", //5
    "AccZ            ",
    "Gas             ",
    "Compass Value   ",
    "Voltage         ",
    "Empfang         ", //10
    "Gyro Kompass    ",
    "Motor Front     ",
    "Motor Rear      ",
    "Motor Left      ",
    "Motor Right     ", //15
    "                ",
    "                ",
    "VarioMeter      ",
    "MK3Mag CalState ",
    "Servo           ", //20
    "                ",
    "                ",
    "                ",
    "                ",
    "                ", //25
    "                ",
    "Kalman_MaxDrift ",
    "                ",
    "Navi Serial Data",
    "GPS_Nick        ", //30
    "GPS_Roll        "
};
 
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ 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;
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ 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;
 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 + 1;
     RxdBuffer[buf_ptr] = '\r';
         if(RxdBuffer[2] == 'R')
          {
           wdt_enable(WDTO_250MS); // Reset-Commando
           ServoActive = 0;
          }
        }
  }
  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 address, unsigned char BufferAnzahl, ...) //unsigned char *snd, unsigned char len)
{
 va_list ap;
 unsigned int pt = 0;
 unsigned char a,b,c;
 unsigned char ptr = 0;
 
 unsigned char *snd = 0;
 int len = 0;
 
 SendeBuffer[pt++] = '#';                               // Startzeichen
 SendeBuffer[pt++] = 'a' + address;             // Adresse (a=0; b=1,...)
 SendeBuffer[pt++] = cmd;                               // Commando
 
 va_start(ap, BufferAnzahl);
 if(BufferAnzahl)
 {
                snd = va_arg(ap, unsigned char*);
                len = va_arg(ap, int);
                ptr = 0;
                BufferAnzahl--;
 }
 while(len)
  {
        if(len)
        {
           a = snd[ptr++];
           len--;
           if((!len) && BufferAnzahl)
                {
                        snd = va_arg(ap, unsigned char*);
                        len = va_arg(ap, int);
                        ptr = 0;
                        BufferAnzahl--;
                }
        }
        else a = 0;
        if(len)
        {
                b = snd[ptr++];
                len--;
                if((!len) && BufferAnzahl)
                {
                        snd = va_arg(ap, unsigned char*);
                        len = va_arg(ap, int);
                        ptr = 0;
                        BufferAnzahl--;
                }
        }
        else b = 0;
        if(len)
        {
                c = snd[ptr++];
                len--;
                if((!len) && BufferAnzahl)
                {
                        snd = va_arg(ap, unsigned char*);
                        len = va_arg(ap, int);
                        ptr = 0;
                        BufferAnzahl--;
                }
        }
        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);
  }
 va_end(ap);
 AddCRC(pt);
}
 
 
// --------------------------------------------------------------------------
void Decode64(void)  // die daten werden im rx buffer dekodiert, das geht nur, weil aus 4 byte immer 3 gemacht werden.
{
 unsigned char a,b,c,d;
 unsigned char x,y,z;
 unsigned char ptrIn = 3; // start at begin of data block
 unsigned char ptrOut = 3;
 unsigned char len = AnzahlEmpfangsBytes - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers  ('#',Addr, Cmd) und 3 Bytes des Footers (CRC1, CRC2, '\r') ab.
 
 while(len)
  {
   a = RxdBuffer[ptrIn++] - '=';
   b = RxdBuffer[ptrIn++] - '=';
   c = RxdBuffer[ptrIn++] - '=';
   d = RxdBuffer[ptrIn++] - '=';
 
   x = (a << 2) | (b >> 4);
   y = ((b & 0x0f) << 4) | (c >> 2);
   z = ((c & 0x03) << 6) | d;
 
   if(len--) RxdBuffer[ptrOut++] = x; else break;
   if(len--) RxdBuffer[ptrOut++] = y; else break;
   if(len--) RxdBuffer[ptrOut++] = z;   else break;
  }
        pRxData = (unsigned char*)&RxdBuffer[3]; // decodierte Daten beginnen beim 4. Byte
        RxDataLen = ptrOut - 3;  // wie viele Bytes wurden dekodiert?
 
}
 
// --------------------------------------------------------------------------
void BearbeiteRxDaten(void)
{
 if(!NeuerDatensatzEmpfangen) return;
 
        unsigned char tempchar1, tempchar2;
        Decode64(); // dekodiere datenblock im Empfangsbuffer
        switch(RxdBuffer[1]-'a') // check for Slave Address
        {
                case FC_ADDRESS: // FC special commands
 
                switch(RxdBuffer[2])
                {
                        case 'K':// Kompasswert
                                        memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue));
                                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
                                        break;
                        case 't':// Motortest
                                if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));
                                else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);
                                        PC_MotortestActive = 240;
                                        //while(!UebertragungAbgeschlossen);
                                        //SendOutData('T', MeineSlaveAdresse, 0);
                                        PcZugriff = 255;
                                        break;
 
                        case 'n':// "Get Mixer
                                        while(!UebertragungAbgeschlossen);
                    SendOutData('N', FC_ADDRESS, 1, (unsigned char *) &Mixer,sizeof(Mixer));
                                        break;
 
                        case 'm':// "Write Mixer
                                        while(!UebertragungAbgeschlossen);
                    if(pRxData[0] == MIXER_REVISION)
                                         {
                       memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer));
                       eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
                                           tempchar1 = 1;
                                         }
                     else  tempchar1 = 0;
                                        SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
                                        break;
 
                        case 'p': // get PPM Channels
                                        GetPPMChannelAnforderung = 1;
                                        break;
 
                        case 'q':// "Get"-Anforderung für Settings
                                        // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
                                        if(pRxData[0] == 0xFF)
                                        {
                                                pRxData[0] = GetActiveParamSetNumber();
                                        }
                                        // limit settings range
                                        if(pRxData[0] < 1) pRxData[0] = 1; // limit to 5
                                        else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
                                        // load requested parameter set
                                        ReadParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 
                                        while(!UebertragungAbgeschlossen);
                                        tempchar1 = pRxData[0];
                                        tempchar2 = EE_DATENREVISION;
                                        SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                                        break;
 
                        case 's': // Parametersatz speichern
                                        if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EE_DATENREVISION)) // check for setting to be in range
                                        {
                                                memcpy((unsigned char *) &EE_Parameter.Kanalbelegung[0], (unsigned char *)&pRxData[2], STRUCT_PARAM_LAENGE);
                                                WriteParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                                                Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
                                                Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
                                                SetActiveParamSetNumber(pRxData[0]);
                                                tempchar1 = GetActiveParamSetNumber();
                                                Piep(tempchar1,110);
                                        }
                                        else
                                        {
                                                tempchar1 = 0; // mark in response an invlid setting
                                        }
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
                                        break;
 
                } // case FC_ADDRESS:
 
                default: // any Slave Address
 
                switch(RxdBuffer[2])
                {
                        // 't' comand placed here only for compatibility to BL
                        case 't':// Motortest
                                if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));
                                else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('T', MeineSlaveAdresse, 0);
                                        PC_MotortestActive = 250;
                                        PcZugriff = 255;
                                        break;
                        // 'K' comand placed here only for compatibility to old MK3MAG software, that does not send the right Slave Address
                        case 'K':// Kompasswert
                                        memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue));
                                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
                                        break;
                        case 'a':// Texte der Analogwerte
                                        DebugTextAnforderung = pRxData[0];
                                        if (DebugTextAnforderung > 31) DebugTextAnforderung = 31;
                                        PcZugriff = 255;
                                        break;
                        case 'b':
                                        memcpy((unsigned char *)&ExternControl, (unsigned char *)pRxData, sizeof(ExternControl));
                                        ConfirmFrame = ExternControl.Frame;
                                        PcZugriff = 255;
                                        break;
                        case 'c': // Poll the 3D-Data
                    if(!Intervall3D) { if(pRxData[0]) Timer3D = SetDelay(pRxData[0] * 10);}
                                        Intervall3D = pRxData[0] * 10;
                                        break;
                        case 'd': // Poll the debug data
                                        DebugDataIntervall = pRxData[0] * 10;
                                        if(DebugDataIntervall > 0) DebugDataAnforderung = 1;
                                        break;
 
                        case 'h':// x-1 Displayzeilen
                                PcZugriff = 255;
                                        RemoteKeys |= pRxData[0];
                                        if(RemoteKeys) DisplayLine = 0;
                                        DebugDisplayAnforderung = 1;
                                        break;
 
                        case 'l':// x-1 Displayzeilen
                                PcZugriff = 255;
                                        MenuePunkt = pRxData[0];
                                        DebugDisplayAnforderung1 = 1;
                                        break;
                        case 'v': // Version-Anforderung und Ausbaustufe
                                        GetVersionAnforderung = 1;
                                        break;
 
                        case 'g'://
                                        GetExternalControl = 1;
                                        break;
                }
                break; // default:
        }
        NeuerDatensatzEmpfangen = 0;
        pRxData = 0;
        RxDataLen = 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(DebugDataIntervall);
        Kompass_Timer = SetDelay(220);
 
        VersionInfo.SWMajor = VERSION_MAJOR;
        VersionInfo.SWMinor = VERSION_MINOR;
        VersionInfo.SWPatch = VERSION_PATCH;
        VersionInfo.ProtoMajor  = VERSION_SERIAL_MAJOR;
        VersionInfo.ProtoMinor  = VERSION_SERIAL_MINOR;
 
        pRxData = 0;
        RxDataLen = 0;
}
 
//---------------------------------------------------------------------------------------------
void DatenUebertragung(void)
{
 if(!UebertragungAbgeschlossen) return;
 
        if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
        {
                Menu();
                SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
                DisplayLine++;
                if(DisplayLine >= 4) DisplayLine = 0;
                DebugDisplayAnforderung = 0;
        }
        if(DebugDisplayAnforderung1 && UebertragungAbgeschlossen)
        {
                Menu();
                SendOutData('L', FC_ADDRESS, 3, &MenuePunkt, sizeof(MenuePunkt), &MaxMenue, sizeof(MaxMenue), DisplayBuff, sizeof(DisplayBuff));
                DebugDisplayAnforderung1 = 0;
        }
        if(GetVersionAnforderung && UebertragungAbgeschlossen)
        {
                SendOutData('V', FC_ADDRESS, 1, (unsigned char *) &VersionInfo, sizeof(VersionInfo));
                GetVersionAnforderung = 0;
        }
 
        if(GetExternalControl && UebertragungAbgeschlossen)           // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
        {
                SendOutData('G',MeineSlaveAdresse, 1, (unsigned char *) &ExternControl, sizeof(ExternControl));
                GetExternalControl = 0;
        }
    if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen)
         {
                  WinkelOut.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4));  // etwa in 0.1 Grad
                  WinkelOut.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4));  // etwa in 0.1 Grad
                  WinkelOut.UserParameter[0] = Parameter_UserParam1;
                  WinkelOut.UserParameter[1] = Parameter_UserParam2;
          SendOutData('w', MK3MAG_ADDRESS, 1, (unsigned char *) &WinkelOut,sizeof(WinkelOut));
          if(WinkelOut.CalcState > 4)  WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt
          Kompass_Timer = SetDelay(99);
         }
    if(((DebugDataIntervall>0 && CheckDelay(Debug_Timer)) || DebugDataAnforderung) && UebertragungAbgeschlossen)
         {
          SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut));
          DebugDataAnforderung = 0;
          if(DebugDataIntervall>0) Debug_Timer = SetDelay(DebugDataIntervall);
         }
    if(Intervall3D > 0 && CheckDelay(Timer3D) && UebertragungAbgeschlossen)
         {
                  Data3D.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4));  // etwa in 0.1 Grad
                  Data3D.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4));  // etwa in 0.1 Grad
          Data3D.Winkel[2] = (int) ((10 * ErsatzKompass) / GIER_GRAD_FAKTOR);
          SendOutData('C', FC_ADDRESS, 1, (unsigned char *) &Data3D,sizeof(Data3D));
          Timer3D = SetDelay(Intervall3D);
         }
    if(DebugTextAnforderung != 255) // Texte für die Analogdaten
     {
      SendOutData('A', FC_ADDRESS, 2, (unsigned char *)&DebugTextAnforderung, sizeof(DebugTextAnforderung),(unsigned char *) ANALOG_TEXT[DebugTextAnforderung], 16);
      DebugTextAnforderung = 255;
         }
     if(ConfirmFrame && UebertragungAbgeschlossen)   // Datensatz bestätigen
         {
                SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
        ConfirmFrame = 0;
     }
 
     if(GetPPMChannelAnforderung && UebertragungAbgeschlossen)
     {
                 SendOutData('P', FC_ADDRESS, 1, (unsigned char *) &PPM_in, sizeof(PPM_in));
                 GetPPMChannelAnforderung = 0;
         }
 
}
 
 

Rev 1232	Rev 1253
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	#include <stdarg.h>	7	#include <stdarg.h>
8	#include <string.h>	8	#include <string.h>
9	#include "main.h"	9	#include "main.h"
10	#include "uart.h"	10	#include "uart.h"
11		11
12		12
13	#define FC_ADDRESS 1	13	#define FC_ADDRESS 1
14	#define NC_ADDRESS 2	14	#define NC_ADDRESS 2
15	#define MK3MAG_ADDRESS 3	15	#define MK3MAG_ADDRESS 3
16		16
17	unsigned char GetExternalControl = 0,DebugDisplayAnforderung1 = 0, DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0, GetPPMChannelAnforderung = 0;	17	unsigned char GetExternalControl = 0,DebugDisplayAnforderung1 = 0, DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0, GetPPMChannelAnforderung = 0;
18	unsigned char DisplayLine = 0;	18	unsigned char DisplayLine = 0;
19	unsigned volatile char SioTmp = 0;	19	unsigned volatile char SioTmp = 0;
20	unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];	20	unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
21	unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];	21	unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
22	unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];	22	unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
23	unsigned volatile char NeuerDatensatzEmpfangen = 0;	23	unsigned volatile char NeuerDatensatzEmpfangen = 0;
24	unsigned volatile char NeueKoordinateEmpfangen = 0;	24	unsigned volatile char NeueKoordinateEmpfangen = 0;
25	unsigned volatile char UebertragungAbgeschlossen = 1;	25	unsigned volatile char UebertragungAbgeschlossen = 1;
26	unsigned volatile char CntCrcError = 0;	26	unsigned volatile char CntCrcError = 0;
27	unsigned volatile char AnzahlEmpfangsBytes = 0;	27	unsigned volatile char AnzahlEmpfangsBytes = 0;
28	unsigned char *pRxData = 0;	28	unsigned char *pRxData = 0;
29	unsigned char RxDataLen = 0;	29	unsigned char RxDataLen = 0;
30	unsigned volatile char PC_DebugTimeout = 0;	30	unsigned volatile char PC_DebugTimeout = 0;
31	unsigned volatile char PC_MotortestActive = 0;	31	unsigned volatile char PC_MotortestActive = 0;
32		32
33	unsigned char DebugTextAnforderung = 255;	33	unsigned char DebugTextAnforderung = 255;
34	unsigned char PcZugriff = 100;	34	unsigned char PcZugriff = 100;
35	unsigned char MotorTest[16];	35	unsigned char MotorTest[16];
36	unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl	36	unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl
37	unsigned char ConfirmFrame;	37	unsigned char ConfirmFrame;
38	struct str_DebugOut DebugOut;	38	struct str_DebugOut DebugOut;
39	struct str_ExternControl ExternControl;	39	struct str_ExternControl ExternControl;
40	struct str_VersionInfo VersionInfo;	40	struct str_VersionInfo VersionInfo;
41	struct str_WinkelOut WinkelOut;	41	struct str_WinkelOut WinkelOut;
42	struct str_Data3D Data3D;	42	struct str_Data3D Data3D;
43		43
44	int Debug_Timer,Kompass_Timer,Timer3D;	44	int Debug_Timer,Kompass_Timer,Timer3D;
45	unsigned int DebugDataIntervall = 200, Intervall3D = 0;	45	unsigned int DebugDataIntervall = 200, Intervall3D = 0;
46		46
47	const unsigned char ANALOG_TEXT[32][16] =	47	const unsigned char ANALOG_TEXT[32][16] =
48	{	48	{
49	//1234567890123456	49	//1234567890123456
50	"AngleNick ", //0	50	"AngleNick ", //0
51	"AngleRoll ",	51	"AngleRoll ",
52	"AccNick ",	52	"AccNick ",
53	"AccRoll ",	53	"AccRoll ",
54	"GyroGier ",	54	"GyroGier ",
55	"Hight Value ", //5	55	"Hight Value ", //5
56	"AccZ ",	56	"AccZ ",
57	"Gas ",	57	"Gas ",
58	"Compass Value ",	58	"Compass Value ",
59	"Voltage ",	59	"Voltage ",
60	"Empfang ", //10	60	"Empfang ", //10
61	"Gyro Kompass ",	61	"Gyro Kompass ",
62	"Motor Front ",	62	"Motor Front ",
63	"Motor Rear ",	63	"Motor Rear ",
64	"Motor Left ",	64	"Motor Left ",
65	"Motor Right ", //15	65	"Motor Right ", //15
66	" ",	66	" ",
67	" ",	67	" ",
68	" ",	68	"VarioMeter ",
69	"MK3Mag CalState ",	69	"MK3Mag CalState ",
70	"Servo ", //20	70	"Servo ", //20
71	" ",	71	" ",
72	" ",	72	" ",
73	" ",	73	" ",
74	" ",	74	" ",
75	" ", //25	75	" ", //25
76	" ",	76	" ",
77	"Kalman_MaxDrift ",	77	"Kalman_MaxDrift ",
78	" ",	78	" ",
79	"Navi Serial Data",	79	"Navi Serial Data",
80	"GPS_Nick ", //30	80	"GPS_Nick ", //30
81	"GPS_Roll "	81	"GPS_Roll "
82	};	82	};
83		83
84		84
85		85
86	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	86	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
87	//++ Sende-Part der Datenübertragung	87	//++ Sende-Part der Datenübertragung
88	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	88	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
89	SIGNAL(INT_VEC_TX)	89	SIGNAL(INT_VEC_TX)
90	{	90	{
91	static unsigned int ptr = 0;	91	static unsigned int ptr = 0;
92	unsigned char tmp_tx;	92	unsigned char tmp_tx;
93	if(!UebertragungAbgeschlossen)	93	if(!UebertragungAbgeschlossen)
94	{	94	{
95	ptr++; // die [0] wurde schon gesendet	95	ptr++; // die [0] wurde schon gesendet
96	tmp_tx = SendeBuffer[ptr];	96	tmp_tx = SendeBuffer[ptr];
97	if((tmp_tx == '\r') \|\| (ptr == MAX_SENDE_BUFF))	97	if((tmp_tx == '\r') \|\| (ptr == MAX_SENDE_BUFF))
98	{	98	{
99	ptr = 0;	99	ptr = 0;
100	UebertragungAbgeschlossen = 1;	100	UebertragungAbgeschlossen = 1;
101	}	101	}
102	UDR = tmp_tx;	102	UDR = tmp_tx;
103	}	103	}
104	else ptr = 0;	104	else ptr = 0;
105	}	105	}
106		106
107	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	107	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
108	//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung	108	//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
109	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	109	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
110	SIGNAL(INT_VEC_RX)	110	SIGNAL(INT_VEC_RX)
111	{	111	{
112	static unsigned int crc;	112	static unsigned int crc;
113	static unsigned char crc1,crc2,buf_ptr;	113	static unsigned char crc1,crc2,buf_ptr;
114	static unsigned char UartState = 0;	114	static unsigned char UartState = 0;
115	unsigned char CrcOkay = 0;	115	unsigned char CrcOkay = 0;
116		116
117	SioTmp = UDR;	117	SioTmp = UDR;
118	if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;	118	if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;
119	if(SioTmp == '\r' && UartState == 2)	119	if(SioTmp == '\r' && UartState == 2)
120	{	120	{
121	UartState = 0;	121	UartState = 0;
122	crc -= RxdBuffer[buf_ptr-2];	122	crc -= RxdBuffer[buf_ptr-2];
123	crc -= RxdBuffer[buf_ptr-1];	123	crc -= RxdBuffer[buf_ptr-1];
124	crc %= 4096;	124	crc %= 4096;
125	crc1 = '=' + crc / 64;	125	crc1 = '=' + crc / 64;
126	crc2 = '=' + crc % 64;	126	crc2 = '=' + crc % 64;
127	CrcOkay = 0;	127	CrcOkay = 0;
128	if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};	128	if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
129	if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet	129	if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
130	{	130	{
131	NeuerDatensatzEmpfangen = 1;	131	NeuerDatensatzEmpfangen = 1;
132	AnzahlEmpfangsBytes = buf_ptr + 1;	132	AnzahlEmpfangsBytes = buf_ptr + 1;
133	RxdBuffer[buf_ptr] = '\r';	133	RxdBuffer[buf_ptr] = '\r';
134	if(RxdBuffer[2] == 'R')	134	if(RxdBuffer[2] == 'R')
135	{	135	{
136	wdt_enable(WDTO_250MS); // Reset-Commando	136	wdt_enable(WDTO_250MS); // Reset-Commando
137	ServoActive = 0;	137	ServoActive = 0;
138	}	138	}
139	}	139	}
140	}	140	}
141	else	141	else
142	switch(UartState)	142	switch(UartState)
143	{	143	{
144	case 0:	144	case 0:
145	if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet	145	if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet
146	buf_ptr = 0;	146	buf_ptr = 0;
147	RxdBuffer[buf_ptr++] = SioTmp;	147	RxdBuffer[buf_ptr++] = SioTmp;
148	crc = SioTmp;	148	crc = SioTmp;
149	break;	149	break;
150	case 1: // Adresse auswerten	150	case 1: // Adresse auswerten
151	UartState++;	151	UartState++;
152	RxdBuffer[buf_ptr++] = SioTmp;	152	RxdBuffer[buf_ptr++] = SioTmp;
153	crc += SioTmp;	153	crc += SioTmp;
154	break;	154	break;
155	case 2: // Eingangsdaten sammeln	155	case 2: // Eingangsdaten sammeln
156	RxdBuffer[buf_ptr] = SioTmp;	156	RxdBuffer[buf_ptr] = SioTmp;
157	if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;	157	if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
158	else UartState = 0;	158	else UartState = 0;
159	crc += SioTmp;	159	crc += SioTmp;
160	break;	160	break;
161	default:	161	default:
162	UartState = 0;	162	UartState = 0;
163	break;	163	break;
164	}	164	}
165	}	165	}
166		166
167		167
168	// --------------------------------------------------------------------------	168	// --------------------------------------------------------------------------
169	void AddCRC(unsigned int wieviele)	169	void AddCRC(unsigned int wieviele)
170	{	170	{
171	unsigned int tmpCRC = 0,i;	171	unsigned int tmpCRC = 0,i;
172	for(i = 0; i < wieviele;i++)	172	for(i = 0; i < wieviele;i++)
173	{	173	{
174	tmpCRC += SendeBuffer[i];	174	tmpCRC += SendeBuffer[i];
175	}	175	}
176	tmpCRC %= 4096;	176	tmpCRC %= 4096;
177	SendeBuffer[i++] = '=' + tmpCRC / 64;	177	SendeBuffer[i++] = '=' + tmpCRC / 64;
178	SendeBuffer[i++] = '=' + tmpCRC % 64;	178	SendeBuffer[i++] = '=' + tmpCRC % 64;
179	SendeBuffer[i++] = '\r';	179	SendeBuffer[i++] = '\r';
180	UebertragungAbgeschlossen = 0;	180	UebertragungAbgeschlossen = 0;
181	UDR = SendeBuffer[0];	181	UDR = SendeBuffer[0];
182	}	182	}
183		183
184		184
185		185
186	// --------------------------------------------------------------------------	186	// --------------------------------------------------------------------------
187	void SendOutData(unsigned char cmd,unsigned char address, unsigned char BufferAnzahl, ...) //unsigned char *snd, unsigned char len)	187	void SendOutData(unsigned char cmd,unsigned char address, unsigned char BufferAnzahl, ...) //unsigned char *snd, unsigned char len)
188	{	188	{
189	va_list ap;	189	va_list ap;
190	unsigned int pt = 0;	190	unsigned int pt = 0;
191	unsigned char a,b,c;	191	unsigned char a,b,c;
192	unsigned char ptr = 0;	192	unsigned char ptr = 0;
193		193
194	unsigned char *snd = 0;	194	unsigned char *snd = 0;
195	int len = 0;	195	int len = 0;
196		196
197	SendeBuffer[pt++] = '#'; // Startzeichen	197	SendeBuffer[pt++] = '#'; // Startzeichen
198	SendeBuffer[pt++] = 'a' + address; // Adresse (a=0; b=1,...)	198	SendeBuffer[pt++] = 'a' + address; // Adresse (a=0; b=1,...)
199	SendeBuffer[pt++] = cmd; // Commando	199	SendeBuffer[pt++] = cmd; // Commando
200		200
201	va_start(ap, BufferAnzahl);	201	va_start(ap, BufferAnzahl);
202	if(BufferAnzahl)	202	if(BufferAnzahl)
203	{	203	{
204	snd = va_arg(ap, unsigned char*);	204	snd = va_arg(ap, unsigned char*);
205	len = va_arg(ap, int);	205	len = va_arg(ap, int);
206	ptr = 0;	206	ptr = 0;
207	BufferAnzahl--;	207	BufferAnzahl--;
208	}	208	}
209	while(len)	209	while(len)
210	{	210	{
211	if(len)	211	if(len)
212	{	212	{
213	a = snd[ptr++];	213	a = snd[ptr++];
214	len--;	214	len--;
215	if((!len) && BufferAnzahl)	215	if((!len) && BufferAnzahl)
216	{	216	{
217	snd = va_arg(ap, unsigned char*);	217	snd = va_arg(ap, unsigned char*);
218	len = va_arg(ap, int);	218	len = va_arg(ap, int);
219	ptr = 0;	219	ptr = 0;
220	BufferAnzahl--;	220	BufferAnzahl--;
221	}	221	}
222	}	222	}
223	else a = 0;	223	else a = 0;
224	if(len)	224	if(len)
225	{	225	{
226	b = snd[ptr++];	226	b = snd[ptr++];
227	len--;	227	len--;
228	if((!len) && BufferAnzahl)	228	if((!len) && BufferAnzahl)
229	{	229	{
230	snd = va_arg(ap, unsigned char*);	230	snd = va_arg(ap, unsigned char*);
231	len = va_arg(ap, int);	231	len = va_arg(ap, int);
232	ptr = 0;	232	ptr = 0;
233	BufferAnzahl--;	233	BufferAnzahl--;
234	}	234	}
235	}	235	}
236	else b = 0;	236	else b = 0;
237	if(len)	237	if(len)
238	{	238	{
239	c = snd[ptr++];	239	c = snd[ptr++];
240	len--;	240	len--;
241	if((!len) && BufferAnzahl)	241	if((!len) && BufferAnzahl)
242	{	242	{
243	snd = va_arg(ap, unsigned char*);	243	snd = va_arg(ap, unsigned char*);
244	len = va_arg(ap, int);	244	len = va_arg(ap, int);
245	ptr = 0;	245	ptr = 0;
246	BufferAnzahl--;	246	BufferAnzahl--;
247	}	247	}
248	}	248	}
249	else c = 0;	249	else c = 0;
250	SendeBuffer[pt++] = '=' + (a >> 2);	250	SendeBuffer[pt++] = '=' + (a >> 2);
251	SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	251	SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
252	SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	252	SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
253	SendeBuffer[pt++] = '=' + ( c & 0x3f);	253	SendeBuffer[pt++] = '=' + ( c & 0x3f);
254	}	254	}
255	va_end(ap);	255	va_end(ap);
256	AddCRC(pt);	256	AddCRC(pt);
257	}	257	}
258		258
259		259
260	// --------------------------------------------------------------------------	260	// --------------------------------------------------------------------------
261	void Decode64(void) // die daten werden im rx buffer dekodiert, das geht nur, weil aus 4 byte immer 3 gemacht werden.	261	void Decode64(void) // die daten werden im rx buffer dekodiert, das geht nur, weil aus 4 byte immer 3 gemacht werden.
262	{	262	{
263	unsigned char a,b,c,d;	263	unsigned char a,b,c,d;
264	unsigned char x,y,z;	264	unsigned char x,y,z;
265	unsigned char ptrIn = 3; // start at begin of data block	265	unsigned char ptrIn = 3; // start at begin of data block
266	unsigned char ptrOut = 3;	266	unsigned char ptrOut = 3;
267	unsigned char len = AnzahlEmpfangsBytes - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers ('#',Addr, Cmd) und 3 Bytes des Footers (CRC1, CRC2, '\r') ab.	267	unsigned char len = AnzahlEmpfangsBytes - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers ('#',Addr, Cmd) und 3 Bytes des Footers (CRC1, CRC2, '\r') ab.
268		268
269	while(len)	269	while(len)
270	{	270	{
271	a = RxdBuffer[ptrIn++] - '=';	271	a = RxdBuffer[ptrIn++] - '=';
272	b = RxdBuffer[ptrIn++] - '=';	272	b = RxdBuffer[ptrIn++] - '=';
273	c = RxdBuffer[ptrIn++] - '=';	273	c = RxdBuffer[ptrIn++] - '=';
274	d = RxdBuffer[ptrIn++] - '=';	274	d = RxdBuffer[ptrIn++] - '=';
275		275
276	x = (a << 2) \| (b >> 4);	276	x = (a << 2) \| (b >> 4);
277	y = ((b & 0x0f) << 4) \| (c >> 2);	277	y = ((b & 0x0f) << 4) \| (c >> 2);
278	z = ((c & 0x03) << 6) \| d;	278	z = ((c & 0x03) << 6) \| d;
279		279
280	if(len--) RxdBuffer[ptrOut++] = x; else break;	280	if(len--) RxdBuffer[ptrOut++] = x; else break;
281	if(len--) RxdBuffer[ptrOut++] = y; else break;	281	if(len--) RxdBuffer[ptrOut++] = y; else break;
282	if(len--) RxdBuffer[ptrOut++] = z; else break;	282	if(len--) RxdBuffer[ptrOut++] = z; else break;
283	}	283	}
284	pRxData = (unsigned char*)&RxdBuffer[3]; // decodierte Daten beginnen beim 4. Byte	284	pRxData = (unsigned char*)&RxdBuffer[3]; // decodierte Daten beginnen beim 4. Byte
285	RxDataLen = ptrOut - 3; // wie viele Bytes wurden dekodiert?	285	RxDataLen = ptrOut - 3; // wie viele Bytes wurden dekodiert?
286		286
287	}	287	}
288		288
289	// --------------------------------------------------------------------------	289	// --------------------------------------------------------------------------
290	void BearbeiteRxDaten(void)	290	void BearbeiteRxDaten(void)
291	{	291	{
292	if(!NeuerDatensatzEmpfangen) return;	292	if(!NeuerDatensatzEmpfangen) return;
293		293
294	unsigned char tempchar1, tempchar2;	294	unsigned char tempchar1, tempchar2;
295	Decode64(); // dekodiere datenblock im Empfangsbuffer	295	Decode64(); // dekodiere datenblock im Empfangsbuffer
296	switch(RxdBuffer[1]-'a') // check for Slave Address	296	switch(RxdBuffer[1]-'a') // check for Slave Address
297	{	297	{
298	case FC_ADDRESS: // FC special commands	298	case FC_ADDRESS: // FC special commands
299		299
300	switch(RxdBuffer[2])	300	switch(RxdBuffer[2])
301	{	301	{
302	case 'K':// Kompasswert	302	case 'K':// Kompasswert
303	memcpy((unsigned char )&KompassValue , (unsigned char )pRxData, sizeof(KompassValue));	303	memcpy((unsigned char )&KompassValue , (unsigned char )pRxData, sizeof(KompassValue));
304	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	304	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
305	break;	305	break;
306	case 't':// Motortest	306	case 't':// Motortest
307	if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));	307	if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));
308	else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);	308	else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);
309	PC_MotortestActive = 240;	309	PC_MotortestActive = 240;
310	//while(!UebertragungAbgeschlossen);	310	//while(!UebertragungAbgeschlossen);
311	//SendOutData('T', MeineSlaveAdresse, 0);	311	//SendOutData('T', MeineSlaveAdresse, 0);
312	PcZugriff = 255;	312	PcZugriff = 255;
313	break;	313	break;
314		314
315	case 'n':// "Get Mixer	315	case 'n':// "Get Mixer
316	while(!UebertragungAbgeschlossen);	316	while(!UebertragungAbgeschlossen);
317	SendOutData('N', FC_ADDRESS, 1, (unsigned char *) &Mixer,sizeof(Mixer));	317	SendOutData('N', FC_ADDRESS, 1, (unsigned char *) &Mixer,sizeof(Mixer));
318	break;	318	break;
319		319
320	case 'm':// "Write Mixer	320	case 'm':// "Write Mixer
321	while(!UebertragungAbgeschlossen);	321	while(!UebertragungAbgeschlossen);
322	if(pRxData[0] == MIXER_REVISION)	322	if(pRxData[0] == MIXER_REVISION)
323	{	323	{
324	memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer));	324	memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer));
325	eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));	325	eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
326	tempchar1 = 1;	326	tempchar1 = 1;
327	}	327	}
328	else tempchar1 = 0;	328	else tempchar1 = 0;
329	SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));	329	SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
330	break;	330	break;
331		331
332	case 'p': // get PPM Channels	332	case 'p': // get PPM Channels
333	GetPPMChannelAnforderung = 1;	333	GetPPMChannelAnforderung = 1;
334	break;	334	break;
335		335
336	case 'q':// "Get"-Anforderung für Settings	336	case 'q':// "Get"-Anforderung für Settings
337	// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen	337	// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
338	if(pRxData[0] == 0xFF)	338	if(pRxData[0] == 0xFF)
339	{	339	{
340	pRxData[0] = GetActiveParamSetNumber();	340	pRxData[0] = GetActiveParamSetNumber();
341	}	341	}
342	// limit settings range	342	// limit settings range
343	if(pRxData[0] < 1) pRxData[0] = 1; // limit to 5	343	if(pRxData[0] < 1) pRxData[0] = 1; // limit to 5
344	else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5	344	else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
345	// load requested parameter set	345	// load requested parameter set
346	ReadParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	346	ReadParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
347		347
348	while(!UebertragungAbgeschlossen);	348	while(!UebertragungAbgeschlossen);
349	tempchar1 = pRxData[0];	349	tempchar1 = pRxData[0];
350	tempchar2 = EE_DATENREVISION;	350	tempchar2 = EE_DATENREVISION;
351	SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	351	SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
352	break;	352	break;
353		353
354	case 's': // Parametersatz speichern	354	case 's': // Parametersatz speichern
355	if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EE_DATENREVISION)) // check for setting to be in range	355	if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EE_DATENREVISION)) // check for setting to be in range
356	{	356	{
357	memcpy((unsigned char ) &EE_Parameter.Kanalbelegung[0], (unsigned char )&pRxData[2], STRUCT_PARAM_LAENGE);	357	memcpy((unsigned char ) &EE_Parameter.Kanalbelegung[0], (unsigned char )&pRxData[2], STRUCT_PARAM_LAENGE);
358	WriteParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	358	WriteParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
359	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;	359	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
360	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;	360	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
361	SetActiveParamSetNumber(pRxData[0]);	361	SetActiveParamSetNumber(pRxData[0]);
362	tempchar1 = GetActiveParamSetNumber();	362	tempchar1 = GetActiveParamSetNumber();
363	Piep(tempchar1,110);	363	Piep(tempchar1,110);
364	}	364	}
365	else	365	else
366	{	366	{
367	tempchar1 = 0; // mark in response an invlid setting	367	tempchar1 = 0; // mark in response an invlid setting
368	}	368	}
369	while(!UebertragungAbgeschlossen);	369	while(!UebertragungAbgeschlossen);
370	SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));	370	SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
371	break;	371	break;
372		372
373	} // case FC_ADDRESS:	373	} // case FC_ADDRESS:
374		374
375	default: // any Slave Address	375	default: // any Slave Address
376		376
377	switch(RxdBuffer[2])	377	switch(RxdBuffer[2])
378	{	378	{
379	// 't' comand placed here only for compatibility to BL	379	// 't' comand placed here only for compatibility to BL
380	case 't':// Motortest	380	case 't':// Motortest
381	if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));	381	if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest));
382	else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);	382	else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4);
383	while(!UebertragungAbgeschlossen);	383	while(!UebertragungAbgeschlossen);
384	SendOutData('T', MeineSlaveAdresse, 0);	384	SendOutData('T', MeineSlaveAdresse, 0);
385	PC_MotortestActive = 250;	385	PC_MotortestActive = 250;
386	PcZugriff = 255;	386	PcZugriff = 255;
387	break;	387	break;
388	// 'K' comand placed here only for compatibility to old MK3MAG software, that does not send the right Slave Address	388	// 'K' comand placed here only for compatibility to old MK3MAG software, that does not send the right Slave Address
389	case 'K':// Kompasswert	389	case 'K':// Kompasswert
390	memcpy((unsigned char )&KompassValue , (unsigned char )pRxData, sizeof(KompassValue));	390	memcpy((unsigned char )&KompassValue , (unsigned char )pRxData, sizeof(KompassValue));
391	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	391	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
392	break;	392	break;
393	case 'a':// Texte der Analogwerte	393	case 'a':// Texte der Analogwerte
394	DebugTextAnforderung = pRxData[0];	394	DebugTextAnforderung = pRxData[0];
395	if (DebugTextAnforderung > 31) DebugTextAnforderung = 31;	395	if (DebugTextAnforderung > 31) DebugTextAnforderung = 31;
396	PcZugriff = 255;	396	PcZugriff = 255;
397	break;	397	break;
398	case 'b':	398	case 'b':
399	memcpy((unsigned char )&ExternControl, (unsigned char )pRxData, sizeof(ExternControl));	399	memcpy((unsigned char )&ExternControl, (unsigned char )pRxData, sizeof(ExternControl));
400	ConfirmFrame = ExternControl.Frame;	400	ConfirmFrame = ExternControl.Frame;
401	PcZugriff = 255;	401	PcZugriff = 255;
402	break;	402	break;
403	case 'c': // Poll the 3D-Data	403	case 'c': // Poll the 3D-Data
404	if(!Intervall3D) { if(pRxData[0]) Timer3D = SetDelay(pRxData[0] * 10);}	404	if(!Intervall3D) { if(pRxData[0]) Timer3D = SetDelay(pRxData[0] * 10);}
405	Intervall3D = pRxData[0] * 10;	405	Intervall3D = pRxData[0] * 10;
406	break;	406	break;
407	case 'd': // Poll the debug data	407	case 'd': // Poll the debug data
408	DebugDataIntervall = pRxData[0] * 10;	408	DebugDataIntervall = pRxData[0] * 10;
409	if(DebugDataIntervall > 0) DebugDataAnforderung = 1;	409	if(DebugDataIntervall > 0) DebugDataAnforderung = 1;
410	break;	410	break;
411		411
412	case 'h':// x-1 Displayzeilen	412	case 'h':// x-1 Displayzeilen
413	PcZugriff = 255;	413	PcZugriff = 255;
414	RemoteKeys \|= pRxData[0];	414	RemoteKeys \|= pRxData[0];
415	if(RemoteKeys) DisplayLine = 0;	415	if(RemoteKeys) DisplayLine = 0;
416	DebugDisplayAnforderung = 1;	416	DebugDisplayAnforderung = 1;
417	break;	417	break;
418		418
419	case 'l':// x-1 Displayzeilen	419	case 'l':// x-1 Displayzeilen
420	PcZugriff = 255;	420	PcZugriff = 255;
421	MenuePunkt = pRxData[0];	421	MenuePunkt = pRxData[0];
422	DebugDisplayAnforderung1 = 1;	422	DebugDisplayAnforderung1 = 1;
423	break;	423	break;
424	case 'v': // Version-Anforderung und Ausbaustufe	424	case 'v': // Version-Anforderung und Ausbaustufe
425	GetVersionAnforderung = 1;	425	GetVersionAnforderung = 1;
426	break;	426	break;
427		427
428	case 'g'://	428	case 'g'://
429	GetExternalControl = 1;	429	GetExternalControl = 1;
430	break;	430	break;
431	}	431	}
432	break; // default:	432	break; // default:
433	}	433	}
434	NeuerDatensatzEmpfangen = 0;	434	NeuerDatensatzEmpfangen = 0;
435	pRxData = 0;	435	pRxData = 0;
436	RxDataLen = 0;	436	RxDataLen = 0;
437	}	437	}
438		438
439	//############################################################################	439	//############################################################################
440	//Routine für die Serielle Ausgabe	440	//Routine für die Serielle Ausgabe
441	int uart_putchar (char c)	441	int uart_putchar (char c)
442	//############################################################################	442	//############################################################################
443	{	443	{
444	if (c == '\n')	444	if (c == '\n')
445	uart_putchar('\r');	445	uart_putchar('\r');
446	//Warten solange bis Zeichen gesendet wurde	446	//Warten solange bis Zeichen gesendet wurde
447	loop_until_bit_is_set(USR, UDRE);	447	loop_until_bit_is_set(USR, UDRE);
448	//Ausgabe des Zeichens	448	//Ausgabe des Zeichens
449	UDR = c;	449	UDR = c;
450		450
451	return (0);	451	return (0);
452	}	452	}
453		453
454	// --------------------------------------------------------------------------	454	// --------------------------------------------------------------------------
455	void WriteProgramData(unsigned int pos, unsigned char wert)	455	void WriteProgramData(unsigned int pos, unsigned char wert)
456	{	456	{
457	//if (ProgramLocation == IN_RAM) Buffer[pos] = wert;	457	//if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
458	// else eeprom_write_byte(&EE_Buffer[pos], wert);	458	// else eeprom_write_byte(&EE_Buffer[pos], wert);
459	// Buffer[pos] = wert;	459	// Buffer[pos] = wert;
460	}	460	}
461		461
462	//############################################################################	462	//############################################################################
463	//INstallation der Seriellen Schnittstelle	463	//INstallation der Seriellen Schnittstelle
464	void UART_Init (void)	464	void UART_Init (void)
465	//############################################################################	465	//############################################################################
466	{	466	{
467	//Enable TXEN im Register UCR TX-Data Enable & RX Enable	467	//Enable TXEN im Register UCR TX-Data Enable & RX Enable
468		468
469	UCR=(1 << TXEN) \| (1 << RXEN);	469	UCR=(1 << TXEN) \| (1 << RXEN);
470	// UART Double Speed (U2X)	470	// UART Double Speed (U2X)
471	USR \|= (1<<U2X);	471	USR \|= (1<<U2X);
472	// RX-Interrupt Freigabe	472	// RX-Interrupt Freigabe
473	UCSRB \|= (1<<RXCIE);	473	UCSRB \|= (1<<RXCIE);
474	// TX-Interrupt Freigabe	474	// TX-Interrupt Freigabe
475	UCSRB \|= (1<<TXCIE);	475	UCSRB \|= (1<<TXCIE);
476		476
477	//Teiler wird gesetzt	477	//Teiler wird gesetzt
478	UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);	478	UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);
479	//UBRR = 33;	479	//UBRR = 33;
480	//öffnet einen Kanal für printf (STDOUT)	480	//öffnet einen Kanal für printf (STDOUT)
481	//fdevopen (uart_putchar, 0);	481	//fdevopen (uart_putchar, 0);
482	//sbi(PORTD,4);	482	//sbi(PORTD,4);
483	Debug_Timer = SetDelay(DebugDataIntervall);	483	Debug_Timer = SetDelay(DebugDataIntervall);
484	Kompass_Timer = SetDelay(220);	484	Kompass_Timer = SetDelay(220);
485		485
486	VersionInfo.SWMajor = VERSION_MAJOR;	486	VersionInfo.SWMajor = VERSION_MAJOR;
487	VersionInfo.SWMinor = VERSION_MINOR;	487	VersionInfo.SWMinor = VERSION_MINOR;
488	VersionInfo.SWPatch = VERSION_PATCH;	488	VersionInfo.SWPatch = VERSION_PATCH;
489	VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	489	VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
490	VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	490	VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
491		491
492	pRxData = 0;	492	pRxData = 0;
493	RxDataLen = 0;	493	RxDataLen = 0;
494	}	494	}
495		495
496	//---------------------------------------------------------------------------------------------	496	//---------------------------------------------------------------------------------------------
497	void DatenUebertragung(void)	497	void DatenUebertragung(void)
498	{	498	{
499	if(!UebertragungAbgeschlossen) return;	499	if(!UebertragungAbgeschlossen) return;
500		500

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_var_motorsmoothing/uart.c @ 2291 - Rev 1232 → 1253