WebSVN - FlightCtrl - Diff - Rev 1775 and 1796 - /branches/dongfang_FC


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
 
#include "eeprom.h"
#include "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "attitude.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
 
 
 
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE   0
#define TRUE    1
//int8_t test __attribute__ ((section (".noinit")));
uint8_t request_VerInfo                 = FALSE;
uint8_t request_ExternalControl         = FALSE;
uint8_t request_Display                 = FALSE;
uint8_t request_Display1                = FALSE;
uint8_t request_DebugData               = FALSE;
uint8_t request_Data3D                  = FALSE;
uint8_t request_DebugLabel              = 255;
uint8_t request_PPMChannels             = FALSE;
uint8_t request_MotorTest               = FALSE;
uint8_t request_variables               = FALSE;
 
uint8_t DisplayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t ReceivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t RxDataLen = 0;
 
uint8_t motorTestActive  = 0;
uint8_t motorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
uint8_t ConfirmFrame;
 
typedef struct {
  int16_t Heading;
} __attribute__((packed)) Heading_t;
 
DebugOut_t              DebugOut;
Data3D_t                Data3D;
UART_VersionInfo_t      UART_VersionInfo;
 
uint16_t DebugData_Timer;
uint16_t Data3D_Timer;
uint16_t DebugData_Interval = 500; // in 1ms
uint16_t Data3D_Interval = 0; // in 1ms
 
#ifdef USE_MK3MAG
int16_t Compass_Timer;
#endif
 
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] = {
    //1234567890123456
    "AnglePitch      ", //0
    "AngleRoll       ",
    "AngleYaw        ",
    "GyroPitch(PID)  ",
    "GyroRoll(PID)   ",
    "GyroYaw         ", //5
    "GyroPitch(AC)   ",
    "GyroRoll(AC)    ",
    "GyroYaw(AC)     ",
    "AccPitch (angle)",
    "AccRoll (angle) ", //10
    "UBat            ",
    "Pitch Term      ",
    "Roll Term       ",
    "Yaw Term        ",
    "Throttle Term   ", //15
    "0th O Corr pitch",
    "0th O Corr roll ",
    "DriftCompDelta P",
    "DriftCompDelta R",
    "ADPitchGyroOffs ", //20
    "ADRollGyroOffs  ",
    "M1              ",
    "M2              ",
    "M3              ",
    "M4              ", //25
    "ControlYaw      ",
    "Airpress. Range ",
    "DriftCompPitch  ",
    "DriftCompRoll   ",
    "AirpressFiltered", //30
    "AirpressADC     "
  };
 
/****************************************************************/
/*              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);
 
  // initialize the debug timer
  DebugData_Timer = SetDelay(DebugData_Interval);
 
  // unlock rxd_buffer
  rxd_buffer_locked = FALSE;
  pRxData = 0;
  RxDataLen = 0;
 
  // no bytes to send
  txd_complete = TRUE;
 
#ifdef USE_MK3MAG
  Compass_Timer = SetDelay(220);
#endif
 
  UART_VersionInfo.SWMajor = VERSION_MAJOR;
  UART_VersionInfo.SWMinor = VERSION_MINOR;
  UART_VersionInfo.SWPatch = VERSION_PATCH;
  UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
  UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
 
  // restore global interrupt flags
  SREG = sreg;
}
 
/****************************************************************/
/* USART0 transmitter ISR                                       */
/****************************************************************/
ISR(USART0_TX_vect) {
  static uint16_t ptr_txd_buffer = 0;
  uint8_t tmp_tx;
  if(!txd_complete) { // transmission not completed
    ptr_txd_buffer++;                    // die [0] wurde schon gesendet
    tmp_tx = txd_buffer[ptr_txd_buffer];
    // if terminating character or end of txd buffer was reached
    if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) {
      ptr_txd_buffer = 0; // reset txd pointer
      txd_complete = 1; // stop transmission
    }
    UDR0 = tmp_tx; // send current byte will trigger this ISR again
  }
  // transmission completed
  else ptr_txd_buffer = 0;
}
 
/****************************************************************/
/* USART0 receiver               ISR                            */
/****************************************************************/
ISR(USART0_RX_vect) {
  static uint16_t crc;
  static uint8_t ptr_rxd_buffer = 0;
  uint8_t crc1, crc2;
  uint8_t c;
 
  c = UDR0;  // catch the received byte
 
 
 
 
 
 
  if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
 
  // the rxd buffer is unlocked
  if((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
    rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
    crc = c; // init crc
  }
#if 0
  else if (ptr_rxd_buffer == 1) { // handle address
    rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
    crc += c; // update crc
  }
#endif
  else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
    if(c != '\r') { // no termination character
      rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
      crc += c; // update crc
    } else { // termination character was received
      // the last 2 bytes are no subject for checksum calculation
      // they are the checksum itself
      crc -= rxd_buffer[ptr_rxd_buffer-2];
      crc -= rxd_buffer[ptr_rxd_buffer-1];
      // calculate checksum from transmitted data
      crc %= 4096;
      crc1 = '=' + crc / 64;
      crc2 = '=' + crc % 64;
      // compare checksum to transmitted checksum bytes
      if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) {
        // checksum valid
        rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
        ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
        rxd_buffer_locked = TRUE;          // lock the rxd buffer
        // if 2nd byte is an 'R' enable watchdog that will result in an reset
        if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
      } else {  // checksum invalid
        rxd_buffer_locked = FALSE; // unlock rxd buffer
      }
      ptr_rxd_buffer = 0; // reset rxd buffer pointer
    }
  } else { // rxd buffer overrun
    ptr_rxd_buffer = 0; // reset rxd buffer
    rxd_buffer_locked = FALSE; // unlock rxd buffer
  }
}
 
// --------------------------------------------------------------------------
void AddCRC(uint16_t datalen) {
  uint16_t tmpCRC = 0, i;
  for(i = 0; i < datalen; i++) {
    tmpCRC += txd_buffer[i];
  }
  tmpCRC %= 4096;
  txd_buffer[i++] = '=' + tmpCRC / 64;
  txd_buffer[i++] = '=' + tmpCRC % 64;
  txd_buffer[i++] = '\r';
  txd_complete = FALSE;
  UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
}
 
// --------------------------------------------------------------------------
// application example:
// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
/*
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
  va_list ap;
  uint16_t txd_bufferIndex = 0;
  uint8_t *currentBuffer;
  uint8_t currentBufferIndex;
  uint16_t lengthOfCurrentBuffer;
  uint8_t shift = 0;
 
  txd_buffer[txd_bufferIndex++] = '#';                  // Start character
  txd_buffer[txd_bufferIndex++] = 'a' + addr;           // Address (a=0; b=1,...)
  txd_buffer[txd_bufferIndex++] = cmd;                  // Command
 
  va_start(ap, numofbuffers);
 
  while(numofbuffers) {
    currentBuffer = va_arg(ap, uint8_t*);
    lengthOfCurrentBuffer = va_arg(ap, int);
    currentBufferIndex = 0;
    // Encode data: 3 bytes of data are encoded into 4 bytes,
    // where the 2 most significant bits are both 0.
    while(currentBufferIndex != lengthOfCurrentBuffer) {
      if (!shift) txd_buffer[txd_bufferIndex] = 0;
      txd_buffer[txd_bufferIndex]  |= currentBuffer[currentBufferIndex] >> (shift + 2);
      txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
      shift += 2;
      if (shift == 6) { shift=0; txd_bufferIndex++; }
      currentBufferIndex++;
    }
  }
  // If the number of data bytes was not divisible by 3, stuff
  //  with 0 pseudodata  until length is again divisible by 3.
  if (shift == 2) {
    // We need to stuff with zero bytes at the end.
    txd_buffer[txd_bufferIndex]  &= 0b00110000;
    txd_buffer[++txd_bufferIndex] = 0;
    shift = 4;
  }
  if (shift == 4) {
    // We need to stuff with zero bytes at the end.
    txd_buffer[txd_bufferIndex++] &= 0b00111100;
    txd_buffer[txd_bufferIndex]    = 0;
  }
  va_end(ap);
  AddCRC(pt); // add checksum after data block and initates the transmission
}
*/
 
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
  va_list ap;
  uint16_t pt = 0;
  uint8_t a,b,c;
  uint8_t ptr = 0;
 
  uint8_t *pdata = 0;
  int len = 0;
 
  txd_buffer[pt++] = '#';                       // Start character
  txd_buffer[pt++] = 'a' + addr;        // Address (a=0; b=1,...)
  txd_buffer[pt++] = cmd;                       // Command
 
  va_start(ap, numofbuffers);
 
  if(numofbuffers) {
    pdata = va_arg(ap, uint8_t*);
    len = va_arg(ap, int);
    ptr = 0;
    numofbuffers--;
  }
 
  while(len){
    if(len) {
      a = pdata[ptr++];
      len--;
      if((!len) && numofbuffers) {
        pdata = va_arg(ap, uint8_t*);
        len = va_arg(ap, int);
        ptr = 0;
        numofbuffers--;
      }
    }
    else a = 0;
    if(len) {
      b = pdata[ptr++];
      len--;
      if((!len) && numofbuffers) {
        pdata = va_arg(ap, uint8_t*);
        len = va_arg(ap, int);
        ptr = 0;
        numofbuffers--;
      }
    } else b = 0;
    if(len) {
      c = pdata[ptr++];
      len--;
      if((!len) && numofbuffers) {
        pdata = va_arg(ap, uint8_t*);
        len = va_arg(ap, int);
        ptr = 0;
        numofbuffers--;
      }
    }
    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);
  }
  va_end(ap);
  AddCRC(pt); // add checksum after data block and initates the transmission
}
 
// --------------------------------------------------------------------------
void Decode64(void) {
  uint8_t a,b,c,d;
  uint8_t x,y,z;
  uint8_t ptrIn = 3;
  uint8_t ptrOut = 3;
  uint8_t len = ReceivedBytes - 6;
 
  while(len) {
    a = rxd_buffer[ptrIn++] - '=';
    b = rxd_buffer[ptrIn++] - '=';
    c = rxd_buffer[ptrIn++] - '=';
    d = rxd_buffer[ptrIn++] - '=';
    //if(ptrIn > ReceivedBytes - 3) break;
   
    x = (a << 2) | (b >> 4);
    y = ((b & 0x0f) << 4) | (c >> 2);
    z = ((c & 0x03) << 6) | d;
   
    if(len--) rxd_buffer[ptrOut++] = x; else break;
    if(len--) rxd_buffer[ptrOut++] = y; else break;
    if(len--) rxd_buffer[ptrOut++] = z; else break;
  }
  pRxData = &rxd_buffer[3];
  RxDataLen = ptrOut - 3;
}
 
// --------------------------------------------------------------------------
void usart0_ProcessRxData(void) {
  // We control the motorTestActive var from here: Count it down.
  if (motorTestActive) motorTestActive--;
  // if data in the rxd buffer are not locked immediately return
  if(!rxd_buffer_locked) return;
  uint8_t tempchar1, tempchar2;
  Decode64(); // decode data block in rxd_buffer
 
  switch(rxd_buffer[1] - 'a') {
 
  case FC_ADDRESS:
    switch(rxd_buffer[2]) {
#ifdef USE_MK3MAG
    case 'K':// compass value
      compassHeading = ((Heading_t *)pRxData)->Heading;
      compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
      break;
#endif
    case 't': // motor test
      if(RxDataLen > 20) {
        memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest));
      } else {
        memcpy(&motorTest[0], (uint8_t*)pRxData, 4);
      }
      motorTestActive = 255;
      externalControlActive = 255;
      break;
     
    case 'n':// "Get Mixer Table
      while(!txd_complete); // wait for previous frame to be sent
      SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
      break;
 
    case 'm':// "Set Mixer Table
      if(pRxData[0] == EEMIXER_REVISION) {
        memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
        MixerTable_WriteToEEProm();
        while(!txd_complete); // wait for previous frame to be sent
        tempchar1 = 1;
      } else {
        tempchar1 = 0;
      }
      SendOutData('M', FC_ADDRESS,  1, &tempchar1, 1);
      break;
 
    case 'p': // get PPM channels
      request_PPMChannels = TRUE;
      break;
 
    case 'q':// request settings
      if(pRxData[0] == 0xFF) {
        pRxData[0] = GetParamByte(PID_ACTIVE_SET);
      }
      // limit settings range
      if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1
      else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
      // load requested parameter set
      ParamSet_ReadFromEEProm(pRxData[0]);
      tempchar1 = pRxData[0];
      tempchar2 = EEPARAM_REVISION;
      while(!txd_complete); // wait for previous frame to be sent
      SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, sizeof(staticParams));
      break;
 
    case 's': // save settings
      if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off
        {
          if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings
            {
              memcpy(&staticParams, (uint8_t*)&pRxData[2], sizeof(staticParams));
              ParamSet_WriteToEEProm(pRxData[0]);
              /*
                TODO: Remove this encapsulation breach
                turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L;
                turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L;
              */
              tempchar1 = getActiveParamSet();
              beepNumber(tempchar1);
            }
          else
            {
              tempchar1 = 0;    //indicate bad data
            }
          while(!txd_complete); // wait for previous frame to be sent
          SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
        }
      break;
 
    default:
      //unsupported command received
      break;
    } // case FC_ADDRESS:
 
  default: // any Slave Address
    switch(rxd_buffer[2]) {
      case 'a':// request for labels of the analog debug outputs
        request_DebugLabel = pRxData[0];
        if(request_DebugLabel > 31) request_DebugLabel = 31;
        externalControlActive = 255;
        break;
 
      case 'b': // submit extern control
        memcpy(&externalControl, (uint8_t*)pRxData, sizeof(externalControl));
        ConfirmFrame = externalControl.frame;
        externalControlActive = 255;
        break;
 
      case 'h':// request for display columns
        externalControlActive = 255;
        RemoteKeys |= pRxData[0];
        if(RemoteKeys) DisplayLine = 0;
        request_Display = TRUE;
        break;
 
      case 'l':// request for display columns
        externalControlActive = 255;
        MenuItem = pRxData[0];
        request_Display1 = TRUE;
        break;
 
      case 'v': // request for version and board release
        request_VerInfo = TRUE;
        break;
 
    case 'x':
      request_variables = TRUE;
      break;
 
      case 'g':// get external control data
        request_ExternalControl = TRUE;
        break;
 
      case 'd': // request for the debug data
        DebugData_Interval = (uint16_t) pRxData[0] * 10;
        if(DebugData_Interval > 0) request_DebugData = TRUE;
        break;
 
      case 'c': // request for the 3D data
        Data3D_Interval = (uint16_t) pRxData[0] * 10;
        if(Data3D_Interval > 0) request_Data3D = TRUE;
        break;
 
      default:
        //unsupported command received
        break;
      }
    break; // default:
  }
  // unlock the rxd buffer after processing
  pRxData = 0;
  RxDataLen = 0;
  rxd_buffer_locked = FALSE;
}
 
/************************************************************************/
/* Routine für die Serielle Ausgabe                                     */
/************************************************************************/
int16_t uart_putchar (int8_t c) {
  if (c == '\n')
    uart_putchar('\r');
  // wait until previous character was send
  loop_until_bit_is_set(UCSR0A, UDRE0);
  // send character
  UDR0 = c;
  return (0);
}
 
//---------------------------------------------------------------------------------------------
void usart0_TransmitTxData(void) {
  if(!txd_complete) return;
 
  if(request_VerInfo && txd_complete) {
    SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
    request_VerInfo = FALSE;
  }
 
  if(request_Display && txd_complete) {
    LCD_PrintMenu();
    SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
    DisplayLine++;
    if(DisplayLine >= 4) DisplayLine = 0;
    request_Display = FALSE;
  }
 
  if(request_Display1 && txd_complete) {
    LCD_PrintMenu();
    SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
    request_Display1 = FALSE;
  }
 
  if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten
    uint8_t label[16]; // local sram buffer
    memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer
    SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16);
    request_DebugLabel = 0xFF;
  }
 
  if(ConfirmFrame && txd_complete) {   // Datensatz ohne CRC bestätigen
    SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
    ConfirmFrame = 0;
  }
 
  if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) {
    SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
    DebugData_Timer = SetDelay(DebugData_Interval);
    request_DebugData = FALSE;
  }
 
  if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) {
    SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
    Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
    Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
    Data3D.Heading   = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
    Data3D_Timer = SetDelay(Data3D_Interval);
    request_Data3D = FALSE;
  }
 
  if(request_ExternalControl && txd_complete) {
    SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl));
    request_ExternalControl = FALSE;
  }
 
#ifdef USE_MK3MAG
  if((CheckDelay(Compass_Timer)) && txd_complete) {
    ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL);  // approx. 0.1 deg
    ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL);  // approx. 0.1 deg
    ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
    ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
    ToMk3Mag.CalState = compassCalState;
    SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
    // the last state is 5 and should be send only once to avoid multiple flash writing
    if(compassCalState > 4)  compassCalState = 0;
    Compass_Timer = SetDelay(99);
  }
#endif
 
  if(request_MotorTest && txd_complete) {
    SendOutData('T', FC_ADDRESS, 0);
    request_MotorTest = FALSE;
  }
 
  if(request_PPMChannels && txd_complete) {
    SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in));
    request_PPMChannels = FALSE;
  }
 
  if (request_variables && txd_complete) {
    SendOutData('X', FC_ADDRESS, 1, (uint8_t *)&variables, sizeof(variables));
    request_variables = FALSE;
  }
}
 

Rev 1775	Rev 1796
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + Nur für den privaten Gebrauch	3	// + Nur für den privaten Gebrauch
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt und genannt werden	18	// + eindeutig als Ursprung verlinkt und genannt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25	// + mit unserer Zustimmung zulässig	25	// + mit unserer Zustimmung zulässig
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30	// + this list of conditions and the following disclaimer.	30	// + this list of conditions and the following disclaimer.
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32	// + from this software without specific prior written permission.	32	// + from this software without specific prior written permission.
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34	// + for non-commercial use (directly or indirectly)	34	// + for non-commercial use (directly or indirectly)
35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36	// + with our written permission	36	// + with our written permission
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38	// + clearly linked as origin	38	// + clearly linked as origin
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
48	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	48	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	49	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50	// + POSSIBILITY OF SUCH DAMAGE.	50	// + POSSIBILITY OF SUCH DAMAGE.
51	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	51	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52		52
53	#include <avr/io.h>	53	#include <avr/io.h>
54	#include <avr/interrupt.h>	54	#include <avr/interrupt.h>
55	#include <avr/wdt.h>	55	#include <avr/wdt.h>
56	#include <avr/pgmspace.h>	56	#include <avr/pgmspace.h>
57	#include <stdarg.h>	57	#include <stdarg.h>
58	#include <string.h>	58	#include <string.h>
59		59
60	#include "eeprom.h"	60	#include "eeprom.h"
61	#include "menu.h"	61	#include "menu.h"
62	#include "timer0.h"	62	#include "timer0.h"
63	#include "uart0.h"	63	#include "uart0.h"
64	#include "attitude.h"	64	#include "attitude.h"
65	#include "rc.h"	65	#include "rc.h"
66	#include "externalControl.h"	66	#include "externalControl.h"
67	#include "output.h"	67	#include "output.h"
68		-
69	#if defined (USE_MK3MAG)	-
70	#include "ubx.h"	-
71	#endif	68
72	#ifdef USE_MK3MAG	69	#ifdef USE_MK3MAG
73	#include "mk3mag.h"	70	#include "mk3mag.h"
74	#endif	71	#endif
75		72
76	#define FC_ADDRESS 1	73	#define FC_ADDRESS 1
77	#define NC_ADDRESS 2	74	#define NC_ADDRESS 2
78	#define MK3MAG_ADDRESS 3	75	#define MK3MAG_ADDRESS 3
79		76
80	#define FALSE 0	77	#define FALSE 0
81	#define TRUE 1	78	#define TRUE 1
82	//int8_t test __attribute__ ((section (".noinit")));	79	//int8_t test __attribute__ ((section (".noinit")));
83	uint8_t request_VerInfo = FALSE;	80	uint8_t request_VerInfo = FALSE;
84	uint8_t request_ExternalControl = FALSE;	81	uint8_t request_ExternalControl = FALSE;
85	uint8_t request_Display = FALSE;	82	uint8_t request_Display = FALSE;
86	uint8_t request_Display1 = FALSE;	83	uint8_t request_Display1 = FALSE;
87	uint8_t request_DebugData = FALSE;	84	uint8_t request_DebugData = FALSE;
88	uint8_t request_Data3D = FALSE;	85	uint8_t request_Data3D = FALSE;
89	uint8_t request_DebugLabel = 255;	86	uint8_t request_DebugLabel = 255;
90	uint8_t request_PPMChannels = FALSE;	87	uint8_t request_PPMChannels = FALSE;
91	uint8_t request_MotorTest = FALSE;	88	uint8_t request_MotorTest = FALSE;
92	uint8_t request_variables = FALSE;	89	uint8_t request_variables = FALSE;
93		90
94	uint8_t DisplayLine = 0;	91	uint8_t DisplayLine = 0;
95		92
96	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];	93	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
97	volatile uint8_t rxd_buffer_locked = FALSE;	94	volatile uint8_t rxd_buffer_locked = FALSE;
98	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];	95	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
99	volatile uint8_t txd_complete = TRUE;	96	volatile uint8_t txd_complete = TRUE;
100	volatile uint8_t ReceivedBytes = 0;	97	volatile uint8_t ReceivedBytes = 0;
101	volatile uint8_t *pRxData = 0;	98	volatile uint8_t *pRxData = 0;
102	volatile uint8_t RxDataLen = 0;	99	volatile uint8_t RxDataLen = 0;
103		100
104	uint8_t motorTestActive = 0;	101	uint8_t motorTestActive = 0;
105	uint8_t motorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};	102	uint8_t motorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
106	uint8_t ConfirmFrame;	103	uint8_t ConfirmFrame;
107		104
108	typedef struct {	105	typedef struct {
109	int16_t Heading;	106	int16_t Heading;
110	} __attribute__((packed)) Heading_t;	107	} __attribute__((packed)) Heading_t;
111		108
112	DebugOut_t DebugOut;	109	DebugOut_t DebugOut;
113	Data3D_t Data3D;	110	Data3D_t Data3D;
114	UART_VersionInfo_t UART_VersionInfo;	111	UART_VersionInfo_t UART_VersionInfo;
115		112
116	uint16_t DebugData_Timer;	113	uint16_t DebugData_Timer;
117	uint16_t Data3D_Timer;	114	uint16_t Data3D_Timer;
118	uint16_t DebugData_Interval = 500; // in 1ms	115	uint16_t DebugData_Interval = 500; // in 1ms
119	uint16_t Data3D_Interval = 0; // in 1ms	116	uint16_t Data3D_Interval = 0; // in 1ms
120		117
121	#ifdef USE_MK3MAG	118	#ifdef USE_MK3MAG
122	int16_t Compass_Timer;	119	int16_t Compass_Timer;
123	#endif	120	#endif
124		121
125	// keep lables in flash to save 512 bytes of sram space	122	// keep lables in flash to save 512 bytes of sram space
126	const prog_uint8_t ANALOG_LABEL[32][16] = {	123	const prog_uint8_t ANALOG_LABEL[32][16] = {
127	//1234567890123456	124	//1234567890123456
128	"AnglePitch ", //0	125	"AnglePitch ", //0
129	"AngleRoll ",	126	"AngleRoll ",
130	"AngleYaw ",	127	"AngleYaw ",
131	"GyroPitch(PID) ",	128	"GyroPitch(PID) ",
132	"GyroRoll(PID) ",	129	"GyroRoll(PID) ",
133	"GyroYaw ", //5	130	"GyroYaw ", //5
134	"GyroPitch(AC) ",	131	"GyroPitch(AC) ",
135	"GyroRoll(AC) ",	132	"GyroRoll(AC) ",
136	"GyroYaw(AC) ",	133	"GyroYaw(AC) ",
137	"AccPitch (angle)",	134	"AccPitch (angle)",
138	"AccRoll (angle) ", //10	135	"AccRoll (angle) ", //10
139	"UBat ",	136	"UBat ",
140	"Pitch Term ",	137	"Pitch Term ",
141	"Roll Term ",	138	"Roll Term ",
142	"Yaw Term ",	139	"Yaw Term ",
143	"Throttle Term ", //15	140	"Throttle Term ", //15
144	"0th O Corr pitch",	141	"0th O Corr pitch",
145	"0th O Corr roll ",	142	"0th O Corr roll ",
146	"DriftCompDelta P",	143	"DriftCompDelta P",
147	"DriftCompDelta R",	144	"DriftCompDelta R",
148	"ADPitchGyroOffs ", //20	145	"ADPitchGyroOffs ", //20
149	"ADRollGyroOffs ",	146	"ADRollGyroOffs ",
150	"M1 ",	147	"M1 ",
151	"M2 ",	148	"M2 ",
152	"M3 ",	149	"M3 ",
153	"M4 ", //25	150	"M4 ", //25
154	"ControlYaw ",	151	"ControlYaw ",
155	"Acc Z ",	152	"Airpress. Range ",
156	"DriftCompPitch ",	153	"DriftCompPitch ",
157	"DriftCompRoll ",	154	"DriftCompRoll ",
158	"Pitch Noise ", //30	155	"AirpressFiltered", //30
159	"Roll Noise "	156	"AirpressADC "
160	};	157	};
161		158
162	/****************************************************************/	159	/****************************************************************/
163	/* Initialization of the USART0 */	160	/* Initialization of the USART0 */
164	/****************************************************************/	161	/****************************************************************/
165	void usart0_Init (void) {	162	void usart0_Init (void) {
166	uint8_t sreg = SREG;	163	uint8_t sreg = SREG;
167	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);	164	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
168		165
169	// disable all interrupts before configuration	166	// disable all interrupts before configuration
170	cli();	167	cli();
171		168
172	// disable RX-Interrupt	169	// disable RX-Interrupt
173	UCSR0B &= ~(1 << RXCIE0);	170	UCSR0B &= ~(1 << RXCIE0);
174	// disable TX-Interrupt	171	// disable TX-Interrupt
175	UCSR0B &= ~(1 << TXCIE0);	172	UCSR0B &= ~(1 << TXCIE0);
176		173
177	// set direction of RXD0 and TXD0 pins	174	// set direction of RXD0 and TXD0 pins
178	// set RXD0 (PD0) as an input pin	175	// set RXD0 (PD0) as an input pin
179	PORTD \|= (1 << PORTD0);	176	PORTD \|= (1 << PORTD0);
180	DDRD &= ~(1 << DDD0);	177	DDRD &= ~(1 << DDD0);
181	// set TXD0 (PD1) as an output pin	178	// set TXD0 (PD1) as an output pin
182	PORTD \|= (1 << PORTD1);	179	PORTD \|= (1 << PORTD1);
183	DDRD \|= (1 << DDD1);	180	DDRD \|= (1 << DDD1);
184		181
185	// USART0 Baud Rate Register	182	// USART0 Baud Rate Register
186	// set clock divider	183	// set clock divider
187	UBRR0H = (uint8_t)(ubrr >> 8);	184	UBRR0H = (uint8_t)(ubrr >> 8);
188	UBRR0L = (uint8_t)ubrr;	185	UBRR0L = (uint8_t)ubrr;
189		186
190	// USART0 Control and Status Register A, B, C	187	// USART0 Control and Status Register A, B, C
191		188
192	// enable double speed operation in	189	// enable double speed operation in
193	UCSR0A \|= (1 << U2X0);	190	UCSR0A \|= (1 << U2X0);
194	// enable receiver and transmitter in	191	// enable receiver and transmitter in
195	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);	192	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);
196	// set asynchronous mode	193	// set asynchronous mode
197	UCSR0C &= ~(1 << UMSEL01);	194	UCSR0C &= ~(1 << UMSEL01);
198	UCSR0C &= ~(1 << UMSEL00);	195	UCSR0C &= ~(1 << UMSEL00);
199	// no parity	196	// no parity
200	UCSR0C &= ~(1 << UPM01);	197	UCSR0C &= ~(1 << UPM01);
201	UCSR0C &= ~(1 << UPM00);	198	UCSR0C &= ~(1 << UPM00);
202	// 1 stop bit	199	// 1 stop bit
203	UCSR0C &= ~(1 << USBS0);	200	UCSR0C &= ~(1 << USBS0);
204	// 8-bit	201	// 8-bit
205	UCSR0B &= ~(1 << UCSZ02);	202	UCSR0B &= ~(1 << UCSZ02);
206	UCSR0C \|= (1 << UCSZ01);	203	UCSR0C \|= (1 << UCSZ01);
207	UCSR0C \|= (1 << UCSZ00);	204	UCSR0C \|= (1 << UCSZ00);
208		205
209	// flush receive buffer	206	// flush receive buffer
210	while ( UCSR0A & (1<<RXC0) ) UDR0;	207	while ( UCSR0A & (1<<RXC0) ) UDR0;
211		208
212	// enable interrupts at the end	209	// enable interrupts at the end
213	// enable RX-Interrupt	210	// enable RX-Interrupt
214	UCSR0B \|= (1 << RXCIE0);	211	UCSR0B \|= (1 << RXCIE0);
215	// enable TX-Interrupt	212	// enable TX-Interrupt
216	UCSR0B \|= (1 << TXCIE0);	213	UCSR0B \|= (1 << TXCIE0);
217		214
218	// initialize the debug timer	215	// initialize the debug timer
219	DebugData_Timer = SetDelay(DebugData_Interval);	216	DebugData_Timer = SetDelay(DebugData_Interval);
220		217
221	// unlock rxd_buffer	218	// unlock rxd_buffer
222	rxd_buffer_locked = FALSE;	219	rxd_buffer_locked = FALSE;
223	pRxData = 0;	220	pRxData = 0;
224	RxDataLen = 0;	221	RxDataLen = 0;
225		222
226	// no bytes to send	223	// no bytes to send
227	txd_complete = TRUE;	224	txd_complete = TRUE;
228		225
229	#ifdef USE_MK3MAG	226	#ifdef USE_MK3MAG
230	Compass_Timer = SetDelay(220);	227	Compass_Timer = SetDelay(220);
231	#endif	228	#endif
232		229
233	UART_VersionInfo.SWMajor = VERSION_MAJOR;	230	UART_VersionInfo.SWMajor = VERSION_MAJOR;
234	UART_VersionInfo.SWMinor = VERSION_MINOR;	231	UART_VersionInfo.SWMinor = VERSION_MINOR;
235	UART_VersionInfo.SWPatch = VERSION_PATCH;	232	UART_VersionInfo.SWPatch = VERSION_PATCH;
236	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	233	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
237	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	234	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
238		235
239	// restore global interrupt flags	236	// restore global interrupt flags
240	SREG = sreg;	237	SREG = sreg;
241	}	238	}
242		239
243	/****************************************************************/	240	/****************************************************************/
244	/* USART0 transmitter ISR */	241	/* USART0 transmitter ISR */
245	/****************************************************************/	242	/****************************************************************/
246	ISR(USART0_TX_vect) {	243	ISR(USART0_TX_vect) {
247	static uint16_t ptr_txd_buffer = 0;	244	static uint16_t ptr_txd_buffer = 0;
248	uint8_t tmp_tx;	245	uint8_t tmp_tx;
249	if(!txd_complete) { // transmission not completed	246	if(!txd_complete) { // transmission not completed
250	ptr_txd_buffer++; // die [0] wurde schon gesendet	247	ptr_txd_buffer++; // die [0] wurde schon gesendet
251	tmp_tx = txd_buffer[ptr_txd_buffer];	248	tmp_tx = txd_buffer[ptr_txd_buffer];
252	// if terminating character or end of txd buffer was reached	249	// if terminating character or end of txd buffer was reached
253	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {	250	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {
254	ptr_txd_buffer = 0; // reset txd pointer	251	ptr_txd_buffer = 0; // reset txd pointer
255	txd_complete = 1; // stop transmission	252	txd_complete = 1; // stop transmission
256	}	253	}
257	UDR0 = tmp_tx; // send current byte will trigger this ISR again	254	UDR0 = tmp_tx; // send current byte will trigger this ISR again
258	}	255	}
259	// transmission completed	256	// transmission completed
260	else ptr_txd_buffer = 0;	257	else ptr_txd_buffer = 0;
261	}	258	}
262		259
263	/****************************************************************/	260	/****************************************************************/
264	/* USART0 receiver ISR */	261	/* USART0 receiver ISR */
265	/****************************************************************/	262	/****************************************************************/
266	ISR(USART0_RX_vect) {	263	ISR(USART0_RX_vect) {
267	static uint16_t crc;	264	static uint16_t crc;
268	static uint8_t ptr_rxd_buffer = 0;	265	static uint8_t ptr_rxd_buffer = 0;
269	uint8_t crc1, crc2;	266	uint8_t crc1, crc2;
270	uint8_t c;	267	uint8_t c;
271		268
272	c = UDR0; // catch the received byte	269	c = UDR0; // catch the received byte
273		-
274	#if (defined (USE_MK3MAG))	-
275	// If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.	-
276	if(CPUType != ATMEGA644P) ubx_parser(c);	-
277	#endif	-
278		270
279	if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return	271	if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
280		272
281	// the rxd buffer is unlocked	273	// the rxd buffer is unlocked
282	if((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received	274	if((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
283	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	275	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
284	crc = c; // init crc	276	crc = c; // init crc
285	}	277	}
286	#if 0	278	#if 0
287	else if (ptr_rxd_buffer == 1) { // handle address	279	else if (ptr_rxd_buffer == 1) { // handle address
288	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	280	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
289	crc += c; // update crc	281	crc += c; // update crc
290	}	282	}
291	#endif	283	#endif
292	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes	284	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
293	if(c != '\r') { // no termination character	285	if(c != '\r') { // no termination character
294	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	286	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
295	crc += c; // update crc	287	crc += c; // update crc
296	} else { // termination character was received	288	} else { // termination character was received
297	// the last 2 bytes are no subject for checksum calculation	289	// the last 2 bytes are no subject for checksum calculation
298	// they are the checksum itself	290	// they are the checksum itself
299	crc -= rxd_buffer[ptr_rxd_buffer-2];	291	crc -= rxd_buffer[ptr_rxd_buffer-2];
300	crc -= rxd_buffer[ptr_rxd_buffer-1];	292	crc -= rxd_buffer[ptr_rxd_buffer-1];
301	// calculate checksum from transmitted data	293	// calculate checksum from transmitted data
302	crc %= 4096;	294	crc %= 4096;
303	crc1 = '=' + crc / 64;	295	crc1 = '=' + crc / 64;
304	crc2 = '=' + crc % 64;	296	crc2 = '=' + crc % 64;
305	// compare checksum to transmitted checksum bytes	297	// compare checksum to transmitted checksum bytes
306	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) {	298	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) {
307	// checksum valid	299	// checksum valid
308	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	300	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
309	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes	301	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
310	rxd_buffer_locked = TRUE; // lock the rxd buffer	302	rxd_buffer_locked = TRUE; // lock the rxd buffer
311	// if 2nd byte is an 'R' enable watchdog that will result in an reset	303	// if 2nd byte is an 'R' enable watchdog that will result in an reset
312	if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando	304	if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
313	} else { // checksum invalid	305	} else { // checksum invalid
314	rxd_buffer_locked = FALSE; // unlock rxd buffer	306	rxd_buffer_locked = FALSE; // unlock rxd buffer
315	}	307	}
316	ptr_rxd_buffer = 0; // reset rxd buffer pointer	308	ptr_rxd_buffer = 0; // reset rxd buffer pointer
317	}	309	}
318	} else { // rxd buffer overrun	310	} else { // rxd buffer overrun
319	ptr_rxd_buffer = 0; // reset rxd buffer	311	ptr_rxd_buffer = 0; // reset rxd buffer
320	rxd_buffer_locked = FALSE; // unlock rxd buffer	312	rxd_buffer_locked = FALSE; // unlock rxd buffer
321	}	313	}
322	}	314	}
323		315
324	// --------------------------------------------------------------------------	316	// --------------------------------------------------------------------------
325	void AddCRC(uint16_t datalen) {	317	void AddCRC(uint16_t datalen) {
326	uint16_t tmpCRC = 0, i;	318	uint16_t tmpCRC = 0, i;
327	for(i = 0; i < datalen; i++) {	319	for(i = 0; i < datalen; i++) {
328	tmpCRC += txd_buffer[i];	320	tmpCRC += txd_buffer[i];
329	}	321	}
330	tmpCRC %= 4096;	322	tmpCRC %= 4096;
331	txd_buffer[i++] = '=' + tmpCRC / 64;	323	txd_buffer[i++] = '=' + tmpCRC / 64;
332	txd_buffer[i++] = '=' + tmpCRC % 64;	324	txd_buffer[i++] = '=' + tmpCRC % 64;
333	txd_buffer[i++] = '\r';	325	txd_buffer[i++] = '\r';
334	txd_complete = FALSE;	326	txd_complete = FALSE;
335	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)	327	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
336	}	328	}
337		329
338	// --------------------------------------------------------------------------	330	// --------------------------------------------------------------------------
339	// application example:	331	// application example:
340	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);	332	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
341	/*	333	/*
342	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	334	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
343	va_list ap;	335	va_list ap;
344	uint16_t txd_bufferIndex = 0;	336	uint16_t txd_bufferIndex = 0;
345	uint8_t *currentBuffer;	337	uint8_t *currentBuffer;
346	uint8_t currentBufferIndex;	338	uint8_t currentBufferIndex;
347	uint16_t lengthOfCurrentBuffer;	339	uint16_t lengthOfCurrentBuffer;
348	uint8_t shift = 0;	340	uint8_t shift = 0;
349		341
350	txd_buffer[txd_bufferIndex++] = '#'; // Start character	342	txd_buffer[txd_bufferIndex++] = '#'; // Start character
351	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)	343	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)
352	txd_buffer[txd_bufferIndex++] = cmd; // Command	344	txd_buffer[txd_bufferIndex++] = cmd; // Command
353		345
354	va_start(ap, numofbuffers);	346	va_start(ap, numofbuffers);
355		347
356	while(numofbuffers) {	348	while(numofbuffers) {
357	currentBuffer = va_arg(ap, uint8_t*);	349	currentBuffer = va_arg(ap, uint8_t*);
358	lengthOfCurrentBuffer = va_arg(ap, int);	350	lengthOfCurrentBuffer = va_arg(ap, int);
359	currentBufferIndex = 0;	351	currentBufferIndex = 0;
360	// Encode data: 3 bytes of data are encoded into 4 bytes,	352	// Encode data: 3 bytes of data are encoded into 4 bytes,
361	// where the 2 most significant bits are both 0.	353	// where the 2 most significant bits are both 0.
362	while(currentBufferIndex != lengthOfCurrentBuffer) {	354	while(currentBufferIndex != lengthOfCurrentBuffer) {
363	if (!shift) txd_buffer[txd_bufferIndex] = 0;	355	if (!shift) txd_buffer[txd_bufferIndex] = 0;
364	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);	356	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);
365	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;	357	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
366	shift += 2;	358	shift += 2;
367	if (shift == 6) { shift=0; txd_bufferIndex++; }	359	if (shift == 6) { shift=0; txd_bufferIndex++; }
368	currentBufferIndex++;	360	currentBufferIndex++;
369	}	361	}
370	}	362	}
371	// If the number of data bytes was not divisible by 3, stuff	363	// If the number of data bytes was not divisible by 3, stuff
372	// with 0 pseudodata until length is again divisible by 3.	364	// with 0 pseudodata until length is again divisible by 3.
373	if (shift == 2) {	365	if (shift == 2) {
374	// We need to stuff with zero bytes at the end.	366	// We need to stuff with zero bytes at the end.
375	txd_buffer[txd_bufferIndex] &= 0b00110000;	367	txd_buffer[txd_bufferIndex] &= 0b00110000;
376	txd_buffer[++txd_bufferIndex] = 0;	368	txd_buffer[++txd_bufferIndex] = 0;
377	shift = 4;	369	shift = 4;
378	}	370	}
379	if (shift == 4) {	371	if (shift == 4) {
380	// We need to stuff with zero bytes at the end.	372	// We need to stuff with zero bytes at the end.
381	txd_buffer[txd_bufferIndex++] &= 0b00111100;	373	txd_buffer[txd_bufferIndex++] &= 0b00111100;
382	txd_buffer[txd_bufferIndex] = 0;	374	txd_buffer[txd_bufferIndex] = 0;
383	}	375	}
384	va_end(ap);	376	va_end(ap);
385	AddCRC(pt); // add checksum after data block and initates the transmission	377	AddCRC(pt); // add checksum after data block and initates the transmission
386	}	378	}
387	*/	379	*/
388		380
389	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	381	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
390	va_list ap;	382	va_list ap;
391	uint16_t pt = 0;	383	uint16_t pt = 0;
392	uint8_t a,b,c;	384	uint8_t a,b,c;
393	uint8_t ptr = 0;	385	uint8_t ptr = 0;
394		386
395	uint8_t *pdata = 0;	387	uint8_t *pdata = 0;
396	int len = 0;	388	int len = 0;
397		389
398	txd_buffer[pt++] = '#'; // Start character	390	txd_buffer[pt++] = '#'; // Start character
399	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)	391	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
400	txd_buffer[pt++] = cmd; // Command	392	txd_buffer[pt++] = cmd; // Command
401		393
402	va_start(ap, numofbuffers);	394	va_start(ap, numofbuffers);
403		395
404	if(numofbuffers) {	396	if(numofbuffers) {
405	pdata = va_arg(ap, uint8_t*);	397	pdata = va_arg(ap, uint8_t*);
406	len = va_arg(ap, int);	398	len = va_arg(ap, int);
407	ptr = 0;	399	ptr = 0;
408	numofbuffers--;	400	numofbuffers--;
409	}	401	}
410		402
411	while(len){	403	while(len){
412	if(len) {	404	if(len) {
413	a = pdata[ptr++];	405	a = pdata[ptr++];
414	len--;	406	len--;
415	if((!len) && numofbuffers) {	407	if((!len) && numofbuffers) {
416	pdata = va_arg(ap, uint8_t*);	408	pdata = va_arg(ap, uint8_t*);
417	len = va_arg(ap, int);	409	len = va_arg(ap, int);
418	ptr = 0;	410	ptr = 0;
419	numofbuffers--;	411	numofbuffers--;
420	}	412	}
421	}	413	}
422	else a = 0;	414	else a = 0;
423	if(len) {	415	if(len) {
424	b = pdata[ptr++];	416	b = pdata[ptr++];
425	len--;	417	len--;
426	if((!len) && numofbuffers) {	418	if((!len) && numofbuffers) {
427	pdata = va_arg(ap, uint8_t*);	419	pdata = va_arg(ap, uint8_t*);
428	len = va_arg(ap, int);	420	len = va_arg(ap, int);
429	ptr = 0;	421	ptr = 0;
430	numofbuffers--;	422	numofbuffers--;
431	}	423	}
432	} else b = 0;	424	} else b = 0;
433	if(len) {	425	if(len) {
434	c = pdata[ptr++];	426	c = pdata[ptr++];
435	len--;	427	len--;
436	if((!len) && numofbuffers) {	428	if((!len) && numofbuffers) {
437	pdata = va_arg(ap, uint8_t*);	429	pdata = va_arg(ap, uint8_t*);
438	len = va_arg(ap, int);	430	len = va_arg(ap, int);
439	ptr = 0;	431	ptr = 0;
440	numofbuffers--;	432	numofbuffers--;
441	}	433	}
442	}	434	}
443	else c = 0;	435	else c = 0;
444	txd_buffer[pt++] = '=' + (a >> 2);	436	txd_buffer[pt++] = '=' + (a >> 2);
445	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	437	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
446	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	438	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
447	txd_buffer[pt++] = '=' + ( c & 0x3f);	439	txd_buffer[pt++] = '=' + ( c & 0x3f);
448	}	440	}
449	va_end(ap);	441	va_end(ap);
450	AddCRC(pt); // add checksum after data block and initates the transmission	442	AddCRC(pt); // add checksum after data block and initates the transmission
451	}	443	}
452		444
453	// --------------------------------------------------------------------------	445	// --------------------------------------------------------------------------
454	void Decode64(void) {	446	void Decode64(void) {
455	uint8_t a,b,c,d;	447	uint8_t a,b,c,d;
456	uint8_t x,y,z;	448	uint8_t x,y,z;
457	uint8_t ptrIn = 3;	449	uint8_t ptrIn = 3;
458	uint8_t ptrOut = 3;	450	uint8_t ptrOut = 3;
459	uint8_t len = ReceivedBytes - 6;	451	uint8_t len = ReceivedBytes - 6;
460		452
461	while(len) {	453	while(len) {
462	a = rxd_buffer[ptrIn++] - '=';	454	a = rxd_buffer[ptrIn++] - '=';
463	b = rxd_buffer[ptrIn++] - '=';	455	b = rxd_buffer[ptrIn++] - '=';
464	c = rxd_buffer[ptrIn++] - '=';	456	c = rxd_buffer[ptrIn++] - '=';
465	d = rxd_buffer[ptrIn++] - '=';	457	d = rxd_buffer[ptrIn++] - '=';
466	//if(ptrIn > ReceivedBytes - 3) break;	458	//if(ptrIn > ReceivedBytes - 3) break;
467		459
468	x = (a << 2) \| (b >> 4);	460	x = (a << 2) \| (b >> 4);
469	y = ((b & 0x0f) << 4) \| (c >> 2);	461	y = ((b & 0x0f) << 4) \| (c >> 2);
470	z = ((c & 0x03) << 6) \| d;	462	z = ((c & 0x03) << 6) \| d;
471		463
472	if(len--) rxd_buffer[ptrOut++] = x; else break;	464	if(len--) rxd_buffer[ptrOut++] = x; else break;
473	if(len--) rxd_buffer[ptrOut++] = y; else break;	465	if(len--) rxd_buffer[ptrOut++] = y; else break;
474	if(len--) rxd_buffer[ptrOut++] = z; else break;	466	if(len--) rxd_buffer[ptrOut++] = z; else break;
475	}	467	}
476	pRxData = &rxd_buffer[3];	468	pRxData = &rxd_buffer[3];
477	RxDataLen = ptrOut - 3;	469	RxDataLen = ptrOut - 3;
478	}	470	}
479		471
480	// --------------------------------------------------------------------------	472	// --------------------------------------------------------------------------
481	void usart0_ProcessRxData(void) {	473	void usart0_ProcessRxData(void) {
482	// We control the motorTestActive var from here: Count it down.	474	// We control the motorTestActive var from here: Count it down.
483	if (motorTestActive) motorTestActive--;	475	if (motorTestActive) motorTestActive--;
484	// if data in the rxd buffer are not locked immediately return	476	// if data in the rxd buffer are not locked immediately return
485	if(!rxd_buffer_locked) return;	477	if(!rxd_buffer_locked) return;
486	uint8_t tempchar1, tempchar2;	478	uint8_t tempchar1, tempchar2;
487	Decode64(); // decode data block in rxd_buffer	479	Decode64(); // decode data block in rxd_buffer
488		480
489	switch(rxd_buffer[1] - 'a') {	481	switch(rxd_buffer[1] - 'a') {
490		482
491	case FC_ADDRESS:	483	case FC_ADDRESS:
492	switch(rxd_buffer[2]) {	484	switch(rxd_buffer[2]) {
493	#ifdef USE_MK3MAG	485	#ifdef USE_MK3MAG
494	case 'K':// compass value	486	case 'K':// compass value
495	compassHeading = ((Heading_t *)pRxData)->Heading;	487	compassHeading = ((Heading_t *)pRxData)->Heading;
496	compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;	488	compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
497	break;	489	break;
498	#endif	490	#endif
499	case 't': // motor test	491	case 't': // motor test
500	if(RxDataLen > 20) {	492	if(RxDataLen > 20) {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c @ 1816 - Rev 1775 → 1796