WebSVN - FlightCtrl - Diff - Rev 1638 and 1639


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdarg.h>
#include <string.h>
#include <avr/pgmspace.h>
#include "main.h"
#include "uart.h"
#include "libfc.h"
#include "eeprom.h"
 
 
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
#define BL_CTRL_ADDRESS 5
 
#define ABO_TIMEOUT 4000 // disable abo after 4 seconds
#define MAX_SENDE_BUFF     160
#define MAX_EMPFANGS_BUFF  160
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 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 TxdBuffer[MAX_SENDE_BUFF];
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
 
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 Display_Timer, Debug_Timer,Kompass_Timer,Timer3D;
unsigned int DebugDataIntervall = 0, Intervall3D = 0, Display_Interval = 0;
unsigned int AboTimeOut = 0;
 
const unsigned char ANALOG_TEXT[32][16] PROGMEM =
{
   //1234567890123456
    "AngleNick       ", //0
    "AngleRoll       ",
    "AccNick         ",
    "AccRoll         ",
    "YawGyro         ",
    "Height Value    ", //5
    "AccZ            ",
    "Gas             ",
    "Compass Value   ",
    "Voltage [0.1V]  ",
    "Receiver Level  ", //10
    "Gyro Compass    ",
    "Motor 1         ",
    "Motor 2         ",
    "Motor 3         ",
    "Motor 4         ", //15
    "                ",
    "                ",
    "                ",
    "                ",
    "Servo           ", //20
    "Hovergas        ",
    "Current [0.1A]  ",
    "Capacity [mAh]  ",
    "                ",
    "                ", //25
    "                ",
    "                ",
    "I2C-Error       ",
    "                ", //    "Navi Serial Data",
    "GPS_Nick        ", //30
    "GPS_Roll        "
};
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Sende-Part der Datenübertragung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ISR(USART0_TX_vect)
{
 static unsigned int ptr = 0;
 unsigned char tmp_tx;
 if(!UebertragungAbgeschlossen)
  {
   ptr++;                    // die [0] wurde schon gesendet
   tmp_tx = TxdBuffer[ptr];
   if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
    {
     ptr = 0;
     UebertragungAbgeschlossen = 1;
    }
   UDR0 = tmp_tx;
  }
  else ptr = 0;
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ISR(USART0_RX_vect)
{
 static unsigned int crc;
 static unsigned char crc1,crc2,buf_ptr;
 static unsigned char UartState = 0;
 unsigned char CrcOkay = 0;
 
 SioTmp = UDR0;
 if(buf_ptr >= MAX_SENDE_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')
          {
           LcdClear();
           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 += TxdBuffer[i];
  }
   tmpCRC %= 4096;
   TxdBuffer[i++] = '=' + tmpCRC / 64;
   TxdBuffer[i++] = '=' + tmpCRC % 64;
   TxdBuffer[i++] = '\r';
  UebertragungAbgeschlossen = 0;
  UDR0 = TxdBuffer[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;
 
 TxdBuffer[pt++] = '#';                         // Startzeichen
 TxdBuffer[pt++] = 'a' + address;               // Adresse (a=0; b=1,...)
 TxdBuffer[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;
   TxdBuffer[pt++] = '=' + (a >> 2);
   TxdBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
   TxdBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
   TxdBuffer[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;
        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) - 1);
                                        Debug("Mixer lesen");
                                        break;
 
                        case 'm':// "Write Mixer
                    if(pRxData[0] == EEMIXER_REVISION)
                                        {
                       memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer) - 1);
                       MixerTable_WriteToEEProm();
                                           tempchar1 = 1;
                                        }
                    else
                    {
                                                tempchar1 = 0;
                                        }
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
                                       
                                        break;
 
                        case 'p': // get PPM Channels
                                        GetPPMChannelAnforderung = 1;
                                        PcZugriff = 255;
                                        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] = GetActiveParamSet();
                                        }
                                        // 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
                                        ParamSet_ReadFromEEProm(pRxData[0]);
                                        tempchar1 = pRxData[0];
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('Q', FC_ADDRESS, 2, &tempchar1, sizeof(tempchar1), (unsigned char *) &EE_Parameter, sizeof(EE_Parameter) - 1);
                                        Debug("Lese Setting %d", tempchar1);
                                       
                                        break;
 
                        case 's': // Parametersatz speichern
                                        if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range
                                        {
                                                memcpy(&EE_Parameter, (uint8_t*)&pRxData[1], sizeof(EE_Parameter) - 1);
                                                ParamSet_WriteToEEProm(pRxData[0]);
                                                Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
                                                Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
                                                tempchar1 = GetActiveParamSet();
                                        }
                                        else
                                        {
                                                tempchar1 = 0; // mark in response an invlid setting
                                        }
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
                                        if(!MotorenEin) Piep(tempchar1,110);
                                        LipoDetection(0);
                                        LIBFC_ReceiverInit(EE_Parameter.Receiver);
                                        break;
                        case 'f': // auf anderen Parametersatz umschalten
                                if((1 <= pRxData[0]) && (pRxData[0] <= 5)) ParamSet_ReadFromEEProm(pRxData[0]);
                                        tempchar1 = GetActiveParamSet();
                                        while(!UebertragungAbgeschlossen);
                                        SendOutData('F', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
                                        if(!MotorenEin) Piep(tempchar1,110);
                                        LipoDetection(0);
                                        LIBFC_ReceiverInit(EE_Parameter.Receiver);
                                        break;
                        case 'y':// serial Potis
                                        PPM_in[13] = (signed char) pRxData[0]; PPM_in[14] = (signed char) pRxData[1]; PPM_in[15] = (signed char) pRxData[2]; PPM_in[16] = (signed char) pRxData[3];
                                        PPM_in[17] = (signed char) pRxData[4]; PPM_in[18] = (signed char) pRxData[5]; PPM_in[19] = (signed char) pRxData[6]; PPM_in[20] = (signed char) pRxData[7];
                                        PPM_in[21] = (signed char) pRxData[8]; PPM_in[22] = (signed char) pRxData[9]; PPM_in[23] = (signed char) pRxData[10]; PPM_in[24] = (signed char) pRxData[11];
                                        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;
                                        AboTimeOut = SetDelay(ABO_TIMEOUT);
                                        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;
                                        AboTimeOut = SetDelay(ABO_TIMEOUT);
                                        break;
                        case 'd': // Poll the debug data
                                        PcZugriff = 255;
                                        DebugDataIntervall = (unsigned int)pRxData[0] * 10;
                                        if(DebugDataIntervall > 0) DebugDataAnforderung = 1;
                                        AboTimeOut = SetDelay(ABO_TIMEOUT);
                                        break;
 
                        case 'h':// x-1 Displayzeilen
                                PcZugriff = 255;
                                if((pRxData[0] & 0x80) == 0x00) // old format
                                        {
                                                DisplayLine = 2;
                                                Display_Interval = 0;
                                        }
                                        else // new format
                                        {
                                                RemoteKeys |= ~pRxData[0];
                                                Display_Interval = (unsigned int)pRxData[1] * 10;
                                                DisplayLine = 4;
                                                AboTimeOut = SetDelay(ABO_TIMEOUT);
                                        }
                                        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
void uart_putchar (char c)
//############################################################################
{
        //Warten solange bis Zeichen gesendet wurde
        loop_until_bit_is_set(UCSR0A, UDRE0);
        //Ausgabe des Zeichens
        UDR0 = c;
}
 
 
//############################################################################
//INstallation der Seriellen Schnittstelle
void UART_Init (void)
//############################################################################
{
        unsigned int ubrr = (unsigned int) ((unsigned long) F_CPU/(8 * USART0_BAUD) - 1);
 
        //Enable TXEN im Register UCR TX-Data Enable & RX Enable
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
    // UART Double Speed (U2X)
        UCSR0A |= (1 << U2X0);
        // RX-Interrupt Freigabe
        UCSR0B |= (1 << RXCIE0);
        // TX-Interrupt Freigabe
        UCSR0B |= (1 << TXCIE0);
        // USART0 Baud Rate Register
        // set clock divider
        UBRR0H = (uint8_t)(ubrr >> 8);
        UBRR0L = (uint8_t)ubrr;
 
        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(CheckDelay(AboTimeOut))
        {
                Display_Interval = 0;
                DebugDataIntervall = 0;
                Intervall3D = 0;
        }
 
        if(((Display_Interval>0 && CheckDelay(Display_Timer)) || DebugDisplayAnforderung) && UebertragungAbgeschlossen)
        {
                if(DisplayLine > 3)// new format
                {
                        Menu();
                        SendOutData('H', FC_ADDRESS, 1, (uint8_t *)DisplayBuff, 80);
                }
                else // old format
                {
                        LCD_printfxy(0,0,"!!! INCOMPATIBLE !!!");
                        SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), (uint8_t *)DisplayBuff, 20);
                        if(DisplayLine++ > 3) DisplayLine = 0;
                }
                Display_Timer = SetDelay(Display_Interval);
                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;
                Debug_OK("Version gesendet");
        }
 
        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('k', 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)
         {
                  CopyDebugValues();
          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
     {
                unsigned char label[16]; // local sram buffer
                memcpy_P(label, ANALOG_TEXT[DebugTextAnforderung], 16); // read lable from flash to sra
      SendOutData('A', FC_ADDRESS, 2, (unsigned char *)&DebugTextAnforderung, sizeof(DebugTextAnforderung),label, 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;
         }
         
#ifdef DEBUG                                                                                                                    // only include functions if DEBUG is defined
     if(SendDebugOutput && UebertragungAbgeschlossen)
     {
                 SendOutData('0', FC_ADDRESS, 1, (unsigned char *) &tDebug, sizeof(tDebug));
                 SendDebugOutput = 0;
         }
#endif   
 
}
 
 
 

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

Subversion Repositories FlightCtrl

(root)/tags/V0.90g/uart.c @ 2321 - Rev 1638 → 1639