Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 04.2007 Holger Buss

// + only for non-profit use

// + www.MikroKopter.com

// + see the File "License.txt" for further Informations

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <avr/io.h>

#include <avr/interrupt.h>

#include <avr/wdt.h>

#include "eeprom.h"

#include "main.h"

#include "menu.h"

#include "timer0.h"

#include "uart.h"

#include "fc.h"

#include "_Settings.h"

#include "rc.h"

unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;

unsigned volatile char SioTmp = 0;

unsigned volatile char txd_buffer[TXD_BUFFER_LEN];

unsigned volatile char rxd_buffer[RXD_BUFFER_LEN];

unsigned volatile char NeuerDatensatzEmpfangen = 0;

unsigned volatile char NeueKoordinateEmpfangen = 0;

unsigned volatile char UebertragungAbgeschlossen = 1;

unsigned volatile char CntCrcError = 0;

unsigned volatile char AnzahlEmpfangsBytes = 0;

unsigned volatile char PC_DebugTimeout = 0;

unsigned char RemotePollDisplayLine = 0;

unsigned char NurKanalAnforderung = 0;

unsigned char DebugTextAnforderung = 255;

unsigned char PcAccess = 100;

unsigned char MotorTest[4] = {0,0,0,0};

unsigned char DubWiseKeys[4] = {0,0,0,0};

unsigned char MeineSlaveAdresse;

unsigned char ConfirmFrame;

struct str_DebugOut    DebugOut;

struct str_ExternControl  ExternControl;

struct str_VersionInfo VersionInfo;

int Debug_Timer;

const unsigned char ANALOG_TEXT[32][16] =

{

   //1234567890123456

    "IntegralPitch   ", //0

    "IntegralRoll    ",

    "AccPitch        ",

    "AccRoll         ",

    "GyroYaw         ",

    "ReadingHight    ", //5

    "AccZ            ",

    "Thrust          ",

    "CompassHeading  ",

    "Voltage         ",

    "Receiver Level  ", //10

    "Analog11        ",

    "Motor_Front     ",

    "Motor_Rear      ",

    "Motor_Left      ",

    "Motor_Right     ", //15

    "Acc_Z           ",

    "MeanAccPitch    ",

    "MeanAccRoll     ",

    "IntegralErrPitch",

    "IntegralErrRoll ", //20

    "MeanIntPitch    ",

    "MMeanIntRoll        ",

    "NeutralPitch    ",

    "RollOffset      ",

    "IntRoll*Factor  ", //25

    "ReadingPitch    ",

    "DirectCorrRoll  ",

    "ReadingRoll     ",

    "CorrectionRoll  ",

    "I-AttRoll       ", //30

    "StickRoll       "

};

/****************************************************************/

/*              Initialization of the USART0                    */

/****************************************************************/

void USART0_Init (void)

{

        uint8_t sreg = SREG;

        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);

        // disable all interrupts before configuration

        cli();

        // disable RX-Interrupt

        UCSR0B &= ~(1 << RXCIE0);

        // disable TX-Interrupt

        UCSR0B &= ~(1 << TXCIE0);

        // set direction of RXD0 and TXD0 pins

        // set RXD0 (PD0) as an input pin

        PORTD |= (1 << PORTD0);

        DDRD &= ~(1 << DDD0);

        // set TXD0 (PD1) as an output pin

        PORTD |= (1 << PORTD1);

        DDRD |=  (1 << DDD1);

        // USART0 Baud Rate Register

        // set clock divider

        UBRR0H = (uint8_t)(ubrr >> 8);

        UBRR0L = (uint8_t)ubrr;

        // USART0 Control and Status Register A, B, C

        // enable double speed operation in

        UCSR0A |= (1 << U2X0);

        // enable receiver and transmitter in

        UCSR0B = (1 << TXEN0) | (1 << RXEN0);

        // set asynchronous mode

        UCSR0C &= ~(1 << UMSEL01);

        UCSR0C &= ~(1 << UMSEL00);

        // no parity

        UCSR0C &= ~(1 << UPM01);

        UCSR0C &= ~(1 << UPM00);

        // 1 stop bit

        UCSR0C &= ~(1 << USBS0);

        // 8-bit

        UCSR0B &= ~(1 << UCSZ02);

        UCSR0C |=  (1 << UCSZ01);

        UCSR0C |=  (1 << UCSZ00);

                // flush receive buffer

        while ( UCSR0A & (1<<RXC0) ) UDR0;

        // enable interrupts at the end

        // enable RX-Interrupt

        UCSR0B |= (1 << RXCIE0);

        // enable TX-Interrupt

        UCSR0B |= (1 << TXCIE0);

        // restore global interrupt flags

    SREG = sreg;

}

/****************************************************************/

/*               USART0 transmitter ISR                         */

/****************************************************************/

ISR(USART0_TX_vect)

{

 static uint16_t ptr = 0;

 uint8_t tmp_tx;

 if(!UebertragungAbgeschlossen)

  {

   ptr++;                    // die [0] wurde schon gesendet

   tmp_tx = txd_buffer[ptr];

   if((tmp_tx == '\r') || (ptr == TXD_BUFFER_LEN))

    {

     ptr = 0;

     UebertragungAbgeschlossen = 1;

    }

   UDR0 = tmp_tx; // send byte will trigger this ISR again

  }

  else ptr = 0;

}

/****************************************************************/

/*               USART0 receiver ISR                            */

/****************************************************************/

ISR(USART0_RX_vect)

{

 static uint16_t crc;

 static uint8_t crc1,crc2,buf_ptr;

 static uint8_t UartState = 0;

 uint8_t CrcOkay = 0;

 SioTmp = UDR0;

 if(buf_ptr >= RXD_BUFFER_LEN)    UartState = 0;

 if(SioTmp == '\r' && UartState == 2)

  {

   UartState = 0;

   crc -= rxd_buffer[buf_ptr-2];

   crc -= rxd_buffer[buf_ptr-1];

   crc %= 4096;

   crc1 = '=' + crc / 64;

   crc2 = '=' + crc % 64;

   CrcOkay = 0;

   if((crc1 == rxd_buffer[buf_ptr-2]) && (crc2 == rxd_buffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};

   if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet

    {

     NeuerDatensatzEmpfangen = 1;

         AnzahlEmpfangsBytes = buf_ptr;

     rxd_buffer[buf_ptr] = '\r';

         if(rxd_buffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando

        }

  }

  else

  switch(UartState)

  {

   case 0:

          if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1;  // Startzeichen und Daten schon verarbeitet

                  buf_ptr = 0;

                  rxd_buffer[buf_ptr++] = SioTmp;

                  crc = SioTmp;

          break;

   case 1: // Adresse auswerten

                  UartState++;

                  rxd_buffer[buf_ptr++] = SioTmp;

                  crc += SioTmp;

                  break;

   case 2: //  Eingangsdaten sammeln

                  rxd_buffer[buf_ptr] = SioTmp;

                  if(buf_ptr < RXD_BUFFER_LEN) buf_ptr++;

                  else UartState = 0;

                  crc += SioTmp;

                  break;

   default:

          UartState = 0;

          break;

  }

}

// --------------------------------------------------------------------------

void AddCRC(unsigned int wieviele)

{

 uint16_t tmpCRC = 0,i;

 for(i = 0; i < wieviele;i++)

  {

   tmpCRC += txd_buffer[i];

  }

   tmpCRC %= 4096;

   txd_buffer[i++] = '=' + tmpCRC / 64;

   txd_buffer[i++] = '=' + tmpCRC % 64;

   txd_buffer[i++] = '\r';

  UebertragungAbgeschlossen = 0;

  UDR0 = txd_buffer[0];

}

// --------------------------------------------------------------------------

void SendOutData(uint8_t cmd,uint8_t modul, uint8_t *snd, uint8_t len)

{

 uint16_t pt = 0;

 uint8_t a,b,c;

 uint8_t ptr = 0;

 txd_buffer[pt++] = '#';               // Startzeichen

 txd_buffer[pt++] = modul;             // Adresse (a=0; b=1,...)

 txd_buffer[pt++] = cmd;                        // Commando

 while(len)

  {

   if(len) { a = snd[ptr++]; len--;} else a = 0;

   if(len) { b = snd[ptr++]; len--;} else b = 0;

   if(len) { c = snd[ptr++]; len--;} else c = 0;

   txd_buffer[pt++] = '=' + (a >> 2);

   txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));

   txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));

   txd_buffer[pt++] = '=' + ( c & 0x3f);

  }

 AddCRC(pt);

}

// --------------------------------------------------------------------------

void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)  // Wohin mit den Daten; Wie lang; Wo im rxd_buffer

{

 uint8_t a,b,c,d;

 uint8_t ptr = 0;

 uint8_t x,y,z;

 while(len)

  {

   a = rxd_buffer[ptrIn++] - '=';

   b = rxd_buffer[ptrIn++] - '=';

   c = rxd_buffer[ptrIn++] - '=';

   d = rxd_buffer[ptrIn++] - '=';

   if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden

   x = (a << 2) | (b >> 4);

   y = ((b & 0x0f) << 4) | (c >> 2);

   z = ((c & 0x03) << 6) | d;

   if(len--) ptrOut[ptr++] = x; else break;

   if(len--) ptrOut[ptr++] = y; else break;

   if(len--) ptrOut[ptr++] = z; else break;

  }

}

// --------------------------------------------------------------------------

void BearbeiteRxDaten(void)

{

 if(!NeuerDatensatzEmpfangen) return;

 uint8_t tmp_char_arr2[2];

 PcAccess = 255;

  switch(rxd_buffer[2])

  {

   case 'a':// Texte der Analogwerte

            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);

            DebugTextAnforderung = tmp_char_arr2[0];

                        break;

   case 'b':

                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);

                        RemoteButtons |= ExternControl.RemoteButtons;

            ConfirmFrame = ExternControl.Frame;

            break;

   case 'c':

                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);

                        RemoteButtons |= ExternControl.RemoteButtons;

            ConfirmFrame = ExternControl.Frame;

            DebugDataAnforderung = 1;

            break;

   case 'h':// x-1 Displayzeilen

            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);

            RemoteButtons |= tmp_char_arr2[0];

                        if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten

                        DebugDisplayAnforderung = 1;

                        break;

   case 't':// Motortest

            Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);

                        break;

   case 'k':// Keys von DubWise

            Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,AnzahlEmpfangsBytes);

                        ConfirmFrame = DubWiseKeys[3];

                        break;

   case 'v': // Version-Anforderung     und Ausbaustufe

            GetVersionAnforderung = 1;

            break;

   case 'g':// "Get"-Anforderung für Debug-Daten

            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen

            DebugGetAnforderung = 1;

            break;

   case 'q':// "Get"-Anforderung für Settings

            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen

            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);

            if(tmp_char_arr2[0] != 0xff)

             {

                          if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;

                  ParamSet_ReadFromEEProm(tmp_char_arr2[0]);

                  SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);

             }

             else

                  SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MeineSlaveAdresse,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);

            break;

   case 'l':

   case 'm':

   case 'n':

   case 'o':

   case 'p': // Parametersatz speichern

            Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,AnzahlEmpfangsBytes);

                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);

            //SetActiveParamSet(rxd_buffer[2] - 'l' + 1);  // is alredy done in ParamSet_WriteToEEProm()

            TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;

            TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;

            Beep(GetActiveParamSet());

         break;

  }

// DebugOut.AnzahlZyklen =  Debug_Timer_Intervall;

 NeuerDatensatzEmpfangen = 0;

}

//############################################################################

//Routine für die Serielle Ausgabe

int16_t uart_putchar (int8_t c)

//############################################################################

{

        if (c == '\n')

                uart_putchar('\r');

        //Warten solange bis Zeichen gesendet wurde

        loop_until_bit_is_set(UCSR0A, UDRE0);

        //Ausgabe des Zeichens

        UDR0 = 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;

}

//---------------------------------------------------------------------------------------------

void DatenUebertragung(void)

{

 if(!UebertragungAbgeschlossen) return;

   if(DebugGetAnforderung && UebertragungAbgeschlossen)               // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen

   {

      SendOutData('G',MeineSlaveAdresse,(uint8_t *) &ExternControl,sizeof(ExternControl));

          DebugGetAnforderung = 0;

   }

    if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)

         {

          SendOutData('D',MeineSlaveAdresse,(uint8_t *) &DebugOut,sizeof(DebugOut));

          DebugDataAnforderung = 0;

          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);

         }

    if(DebugTextAnforderung != 255) // Texte für die Analogdaten

     {

      SendOutData('A',DebugTextAnforderung + '0',(uint8_t *) ANALOG_TEXT[DebugTextAnforderung],16);

      DebugTextAnforderung = 255;

         }

     if(ConfirmFrame && UebertragungAbgeschlossen)   // Datensatz ohne CRC bestätigen

         {

      txd_buffer[0] = '#';

      txd_buffer[1] = ConfirmFrame;

      txd_buffer[2] = '\r';

      UebertragungAbgeschlossen = 0;

      ConfirmFrame = 0;

      UDR0 = txd_buffer[0];

     }

     if(DebugDisplayAnforderung && UebertragungAbgeschlossen)

         {

      LCD_PrintMenu();

          DebugDisplayAnforderung = 0;

      if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)

      {

       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen

       RemotePollDisplayLine = -1;

      }

      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen

         }

    if(GetVersionAnforderung && UebertragungAbgeschlossen)

     {

      SendOutData('V',MeineSlaveAdresse,(uint8_t *) &VersionInfo,sizeof(VersionInfo));

          GetVersionAnforderung = 0;

     }

}