WebSVN - FlightCtrl - Diff - Rev 1980 and 1986 - /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 example: 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 "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.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 = 0; // 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       ",
                "GyroRoll        ",
                "GyroYaw         ", //5
                "AccPitch        ",
                "AccRoll         ",
                "AccZ            ",
                "AccPitch (angle)",
                "AccRoll (angle) ", //10
                "UBat            ",
                "Pitch Term      ",
                "Roll Term       ",
                "Yaw Term        ",
                "Throttle Term   ", //15
                "gyroP           ",
                "ControlAct/10   ",
        "Acc. Vector     ",
        "Max. Acc. Vector",
                "var0            ", //20
                "raw ubat        ",
                "M1              ",
                "M2              ",
                "M3              ",
                "M4              ", //25
                "ControlYaw      ",
                "Airpress. Range ",
                "DriftCompPitch  ",
                "DriftCompRoll   ",
                "Altitude        ", //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 checksum;
        static uint8_t ptr_rxd_buffer = 0;
        uint8_t checksum1, checksum2;
        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
                checksum = c; // init checksum
        }
#if 0
        else if (ptr_rxd_buffer == 1) { // handle address
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
                checksum += c; // update checksum
        }
#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
                        checksum += c; // update checksum
                } else { // termination character was received
                        // the last 2 bytes are no subject for checksum calculation
                        // they are the checksum itself
                        checksum -= rxd_buffer[ptr_rxd_buffer - 2];
                        checksum -= rxd_buffer[ptr_rxd_buffer - 1];
                        // calculate checksum from transmitted data
                        checksum %= 4096;
                        checksum1 = '=' + checksum / 64;
                        checksum2 = '=' + checksum % 64;
                        // compare checksum to transmitted checksum bytes
                        if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2
                                        == 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 Addchecksum(uint16_t datalen) {
        uint16_t tmpchecksum = 0, i;
        for (i = 0; i < datalen; i++) {
                tmpchecksum += txd_buffer[i];
        }
        tmpchecksum %= 4096;
        txd_buffer[i++] = '=' + tmpchecksum / 64;
        txd_buffer[i++] = '=' + tmpchecksum % 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);
 Addchecksum(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);
        Addchecksum(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 tempchar[3];
        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 *) &mixerMatrix, sizeof(mixerMatrix));
                        break;
 
                case 'm':// "Set Mixer Table
                        if (pRxData[0] == EEMIXER_REVISION) {
                                memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix));
                                mixerMatrix_writeToEEProm();
                                while (!txd_complete)
                                        ; // wait for previous frame to be sent
                                tempchar[0] = 1;
                        } else {
                                tempchar[0] = 0;
                        }
                        SendOutData('M', FC_ADDRESS, 1, &tempchar, 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]);
                        tempchar[0] = pRxData[0];
                        tempchar[1] = EEPARAM_REVISION;
                        tempchar[2] = sizeof(staticParams);
                        while (!txd_complete)
                                ; // wait for previous frame to be sent
                        SendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
                        break;
 
                case 's': // save settings
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are 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]);
                                        tempchar[0] = getActiveParamSet();
                                        beepNumber(tempchar[0]);
                                } else {
                                        tempchar[0] = 0; //indicate bad data
                                }
                                while (!txd_complete)
                                        ; // wait for previous frame to be sent
                                SendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
                        }
                        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;
                        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
                        RemoteKeys |= pRxData[0];
                        if (RemoteKeys)
                                DisplayLine = 0;
                        request_Display = TRUE;
                        break;
 
                case 'l':// request for display columns
                        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 checksum 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 1980	Rev 1986
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 example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for example: 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 "rc.h"	64	#include "rc.h"
65	#include "externalControl.h"	65	#include "externalControl.h"
66	#include "output.h"	66	#include "output.h"
67	#include "attitude.h"	67	#include "attitude.h"
68		68
69	#ifdef USE_MK3MAG	69	#ifdef USE_MK3MAG
70	#include "mk3mag.h"	70	#include "mk3mag.h"
71	#endif	71	#endif
72		72
73	#define FC_ADDRESS 1	73	#define FC_ADDRESS 1
74	#define NC_ADDRESS 2	74	#define NC_ADDRESS 2
75	#define MK3MAG_ADDRESS 3	75	#define MK3MAG_ADDRESS 3
76		76
77	#define FALSE 0	77	#define FALSE 0
78	#define TRUE 1	78	#define TRUE 1
79	//int8_t test __attribute__ ((section (".noinit")));	79	//int8_t test __attribute__ ((section (".noinit")));
80	uint8_t request_VerInfo = FALSE;	80	uint8_t request_VerInfo = FALSE;
81	uint8_t request_ExternalControl = FALSE;	81	uint8_t request_ExternalControl = FALSE;
82	uint8_t request_Display = FALSE;	82	uint8_t request_Display = FALSE;
83	uint8_t request_Display1 = FALSE;	83	uint8_t request_Display1 = FALSE;
84	uint8_t request_DebugData = FALSE;	84	uint8_t request_DebugData = FALSE;
85	uint8_t request_Data3D = FALSE;	85	uint8_t request_Data3D = FALSE;
86	uint8_t request_DebugLabel = 255;	86	uint8_t request_DebugLabel = 255;
87	uint8_t request_PPMChannels = FALSE;	87	uint8_t request_PPMChannels = FALSE;
88	uint8_t request_MotorTest = FALSE;	88	uint8_t request_MotorTest = FALSE;
89	uint8_t request_variables = FALSE;	89	uint8_t request_variables = FALSE;
90		90
91	uint8_t DisplayLine = 0;	91	uint8_t DisplayLine = 0;
92		92
93	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];	93	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
94	volatile uint8_t rxd_buffer_locked = FALSE;	94	volatile uint8_t rxd_buffer_locked = FALSE;
95	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];	95	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
96	volatile uint8_t txd_complete = TRUE;	96	volatile uint8_t txd_complete = TRUE;
97	volatile uint8_t ReceivedBytes = 0;	97	volatile uint8_t ReceivedBytes = 0;
98	volatile uint8_t *pRxData = 0;	98	volatile uint8_t *pRxData = 0;
99	volatile uint8_t RxDataLen = 0;	99	volatile uint8_t RxDataLen = 0;
100		100
101	uint8_t motorTestActive = 0;	101	uint8_t motorTestActive = 0;
102	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 };
103	uint8_t ConfirmFrame;	103	uint8_t ConfirmFrame;
104		104
105	typedef struct {	105	typedef struct {
106	int16_t Heading;	106	int16_t Heading;
107	}__attribute__((packed)) Heading_t;	107	}__attribute__((packed)) Heading_t;
108		108
109	DebugOut_t debugOut;	109	DebugOut_t debugOut;
110	Data3D_t Data3D;	110	Data3D_t Data3D;
111	UART_VersionInfo_t UART_VersionInfo;	111	UART_VersionInfo_t UART_VersionInfo;
112		112
113	uint16_t DebugData_Timer;	113	uint16_t DebugData_Timer;
114	uint16_t Data3D_Timer;	114	uint16_t Data3D_Timer;
115	uint16_t DebugData_Interval = 0; // in 1ms	115	uint16_t DebugData_Interval = 0; // in 1ms
116	uint16_t Data3D_Interval = 0; // in 1ms	116	uint16_t Data3D_Interval = 0; // in 1ms
117		117
118	#ifdef USE_MK3MAG	118	#ifdef USE_MK3MAG
119	int16_t Compass_Timer;	119	int16_t Compass_Timer;
120	#endif	120	#endif
121		121
122	// keep lables in flash to save 512 bytes of sram space	122	// keep lables in flash to save 512 bytes of sram space
123	const prog_uint8_t ANALOG_LABEL[32][16] = {	123	const prog_uint8_t ANALOG_LABEL[32][16] = {
124	//1234567890123456	124	//1234567890123456
125	"AnglePitch ", //0	125	"AnglePitch ", //0
126	"AngleRoll ",	126	"AngleRoll ",
127	"AngleYaw ",	127	"AngleYaw ",
128	"GyroPitch ",	128	"GyroPitch ",
129	"GyroRoll ",	129	"GyroRoll ",
130	"GyroYaw ", //5	130	"GyroYaw ", //5
131	"AccPitch ",	131	"AccPitch ",
132	"AccRoll ",	132	"AccRoll ",
133	"AccZ ",	133	"AccZ ",
134	"AccPitch (angle)",	134	"AccPitch (angle)",
135	"AccRoll (angle) ", //10	135	"AccRoll (angle) ", //10
136	"UBat ",	136	"UBat ",
137	"Pitch Term ",	137	"Pitch Term ",
138	"Roll Term ",	138	"Roll Term ",
139	"Yaw Term ",	139	"Yaw Term ",
140	"Throttle Term ", //15	140	"Throttle Term ", //15
141	"gyroP ",	141	"gyroP ",
142	"gyroI ",	142	"ControlAct/10 ",
143	"gyroD ",	143	"Acc. Vector ",
144	"Acc Vector ",	144	"Max. Acc. Vector",
145	"dHeightThrottle ", //20	145	"var0 ", //20
146	"hoverThrottle ",	146	"raw ubat ",
147	"M1 ",	147	"M1 ",
148	"M2 ",	148	"M2 ",
149	"M3 ",	149	"M3 ",
150	"M4 ", //25	150	"M4 ", //25
151	"ControlYaw ",	151	"ControlYaw ",
152	"Airpress. Range ",	152	"Airpress. Range ",
153	"DriftCompPitch ",	153	"DriftCompPitch ",
154	"DriftCompRoll ",	154	"DriftCompRoll ",
155	"Altitude ", //30	155	"Altitude ", //30
156	"AirpressADC " };	156	"AirpressADC " };
157		157
158	/****************************************************************/	158	/****************************************************************/
159	/* Initialization of the USART0 */	159	/* Initialization of the USART0 */
160	/****************************************************************/	160	/****************************************************************/
161	void usart0_Init(void) {	161	void usart0_Init(void) {
162	uint8_t sreg = SREG;	162	uint8_t sreg = SREG;
163	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);	163	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
164		164
165	// disable all interrupts before configuration	165	// disable all interrupts before configuration
166	cli();	166	cli();
167		167
168	// disable RX-Interrupt	168	// disable RX-Interrupt
169	UCSR0B &= ~(1 << RXCIE0);	169	UCSR0B &= ~(1 << RXCIE0);
170	// disable TX-Interrupt	170	// disable TX-Interrupt
171	UCSR0B &= ~(1 << TXCIE0);	171	UCSR0B &= ~(1 << TXCIE0);
172		172
173	// set direction of RXD0 and TXD0 pins	173	// set direction of RXD0 and TXD0 pins
174	// set RXD0 (PD0) as an input pin	174	// set RXD0 (PD0) as an input pin
175	PORTD \|= (1 << PORTD0);	175	PORTD \|= (1 << PORTD0);
176	DDRD &= ~(1 << DDD0);	176	DDRD &= ~(1 << DDD0);
177	// set TXD0 (PD1) as an output pin	177	// set TXD0 (PD1) as an output pin
178	PORTD \|= (1 << PORTD1);	178	PORTD \|= (1 << PORTD1);
179	DDRD \|= (1 << DDD1);	179	DDRD \|= (1 << DDD1);
180		180
181	// USART0 Baud Rate Register	181	// USART0 Baud Rate Register
182	// set clock divider	182	// set clock divider
183	UBRR0H = (uint8_t) (ubrr >> 8);	183	UBRR0H = (uint8_t) (ubrr >> 8);
184	UBRR0L = (uint8_t) ubrr;	184	UBRR0L = (uint8_t) ubrr;
185		185
186	// USART0 Control and Status Register A, B, C	186	// USART0 Control and Status Register A, B, C
187		187
188	// enable double speed operation in	188	// enable double speed operation in
189	UCSR0A \|= (1 << U2X0);	189	UCSR0A \|= (1 << U2X0);
190	// enable receiver and transmitter in	190	// enable receiver and transmitter in
191	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);	191	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);
192	// set asynchronous mode	192	// set asynchronous mode
193	UCSR0C &= ~(1 << UMSEL01);	193	UCSR0C &= ~(1 << UMSEL01);
194	UCSR0C &= ~(1 << UMSEL00);	194	UCSR0C &= ~(1 << UMSEL00);
195	// no parity	195	// no parity
196	UCSR0C &= ~(1 << UPM01);	196	UCSR0C &= ~(1 << UPM01);
197	UCSR0C &= ~(1 << UPM00);	197	UCSR0C &= ~(1 << UPM00);
198	// 1 stop bit	198	// 1 stop bit
199	UCSR0C &= ~(1 << USBS0);	199	UCSR0C &= ~(1 << USBS0);
200	// 8-bit	200	// 8-bit
201	UCSR0B &= ~(1 << UCSZ02);	201	UCSR0B &= ~(1 << UCSZ02);
202	UCSR0C \|= (1 << UCSZ01);	202	UCSR0C \|= (1 << UCSZ01);
203	UCSR0C \|= (1 << UCSZ00);	203	UCSR0C \|= (1 << UCSZ00);
204		204
205	// flush receive buffer	205	// flush receive buffer
206	while (UCSR0A & (1 << RXC0))	206	while (UCSR0A & (1 << RXC0))
207	UDR0;	207	UDR0;
208		208
209	// enable interrupts at the end	209	// enable interrupts at the end
210	// enable RX-Interrupt	210	// enable RX-Interrupt
211	UCSR0B \|= (1 << RXCIE0);	211	UCSR0B \|= (1 << RXCIE0);
212	// enable TX-Interrupt	212	// enable TX-Interrupt
213	UCSR0B \|= (1 << TXCIE0);	213	UCSR0B \|= (1 << TXCIE0);
214		214
215	// initialize the debug timer	215	// initialize the debug timer
216	DebugData_Timer = setDelay(DebugData_Interval);	216	DebugData_Timer = setDelay(DebugData_Interval);
217		217
218	// unlock rxd_buffer	218	// unlock rxd_buffer
219	rxd_buffer_locked = FALSE;	219	rxd_buffer_locked = FALSE;
220	pRxData = 0;	220	pRxData = 0;
221	RxDataLen = 0;	221	RxDataLen = 0;
222		222
223	// no bytes to send	223	// no bytes to send
224	txd_complete = TRUE;	224	txd_complete = TRUE;
225		225
226	#ifdef USE_MK3MAG	226	#ifdef USE_MK3MAG
227	Compass_Timer = setDelay(220);	227	Compass_Timer = setDelay(220);
228	#endif	228	#endif
229		229
230	UART_VersionInfo.SWMajor = VERSION_MAJOR;	230	UART_VersionInfo.SWMajor = VERSION_MAJOR;
231	UART_VersionInfo.SWMinor = VERSION_MINOR;	231	UART_VersionInfo.SWMinor = VERSION_MINOR;
232	UART_VersionInfo.SWPatch = VERSION_PATCH;	232	UART_VersionInfo.SWPatch = VERSION_PATCH;
233	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	233	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
234	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	234	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
235		235
236	// restore global interrupt flags	236	// restore global interrupt flags
237	SREG = sreg;	237	SREG = sreg;
238	}	238	}
239		239
240	/****************************************************************/	240	/****************************************************************/
241	/* USART0 transmitter ISR */	241	/* USART0 transmitter ISR */
242	/****************************************************************/	242	/****************************************************************/
243	ISR(USART0_TX_vect)	243	ISR(USART0_TX_vect)
244	{	244	{
245	static uint16_t ptr_txd_buffer = 0;	245	static uint16_t ptr_txd_buffer = 0;
246	uint8_t tmp_tx;	246	uint8_t tmp_tx;
247	if (!txd_complete) { // transmission not completed	247	if (!txd_complete) { // transmission not completed
248	ptr_txd_buffer++; // die [0] wurde schon gesendet	248	ptr_txd_buffer++; // die [0] wurde schon gesendet
249	tmp_tx = txd_buffer[ptr_txd_buffer];	249	tmp_tx = txd_buffer[ptr_txd_buffer];
250	// if terminating character or end of txd buffer was reached	250	// if terminating character or end of txd buffer was reached
251	if ((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {	251	if ((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {
252	ptr_txd_buffer = 0; // reset txd pointer	252	ptr_txd_buffer = 0; // reset txd pointer
253	txd_complete = 1; // stop transmission	253	txd_complete = 1; // stop transmission
254	}	254	}
255	UDR0 = tmp_tx; // send current byte will trigger this ISR again	255	UDR0 = tmp_tx; // send current byte will trigger this ISR again
256	}	256	}
257	// transmission completed	257	// transmission completed
258	else	258	else
259	ptr_txd_buffer = 0;	259	ptr_txd_buffer = 0;
260	}	260	}
261		261
262	/****************************************************************/	262	/****************************************************************/
263	/* USART0 receiver ISR */	263	/* USART0 receiver ISR */
264	/****************************************************************/	264	/****************************************************************/
265	ISR(USART0_RX_vect)	265	ISR(USART0_RX_vect)
266	{	266	{
267	static uint16_t checksum;	267	static uint16_t checksum;
268	static uint8_t ptr_rxd_buffer = 0;	268	static uint8_t ptr_rxd_buffer = 0;
269	uint8_t checksum1, checksum2;	269	uint8_t checksum1, checksum2;
270	uint8_t c;	270	uint8_t c;
271		271
272	c = UDR0; // catch the received byte	272	c = UDR0; // catch the received byte
273		273
274	if (rxd_buffer_locked)	274	if (rxd_buffer_locked)
275	return; // if rxd buffer is locked immediately return	275	return; // if rxd buffer is locked immediately return
276		276
277	// the rxd buffer is unlocked	277	// the rxd buffer is unlocked
278	if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received	278	if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
279	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	279	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
280	checksum = c; // init checksum	280	checksum = c; // init checksum
281	}	281	}
282	#if 0	282	#if 0
283	else if (ptr_rxd_buffer == 1) { // handle address	283	else if (ptr_rxd_buffer == 1) { // handle address
284	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	284	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
285	checksum += c; // update checksum	285	checksum += c; // update checksum
286	}	286	}
287	#endif	287	#endif
288	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes	288	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
289	if (c != '\r') { // no termination character	289	if (c != '\r') { // no termination character
290	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	290	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
291	checksum += c; // update checksum	291	checksum += c; // update checksum
292	} else { // termination character was received	292	} else { // termination character was received
293	// the last 2 bytes are no subject for checksum calculation	293	// the last 2 bytes are no subject for checksum calculation
294	// they are the checksum itself	294	// they are the checksum itself
295	checksum -= rxd_buffer[ptr_rxd_buffer - 2];	295	checksum -= rxd_buffer[ptr_rxd_buffer - 2];
296	checksum -= rxd_buffer[ptr_rxd_buffer - 1];	296	checksum -= rxd_buffer[ptr_rxd_buffer - 1];
297	// calculate checksum from transmitted data	297	// calculate checksum from transmitted data
298	checksum %= 4096;	298	checksum %= 4096;
299	checksum1 = '=' + checksum / 64;	299	checksum1 = '=' + checksum / 64;
300	checksum2 = '=' + checksum % 64;	300	checksum2 = '=' + checksum % 64;
301	// compare checksum to transmitted checksum bytes	301	// compare checksum to transmitted checksum bytes
302	if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2	302	if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2
303	== rxd_buffer[ptr_rxd_buffer - 1])) {	303	== rxd_buffer[ptr_rxd_buffer - 1])) {
304	// checksum valid	304	// checksum valid
305	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	305	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
306	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes	306	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
307	rxd_buffer_locked = TRUE; // lock the rxd buffer	307	rxd_buffer_locked = TRUE; // lock the rxd buffer
308	// if 2nd byte is an 'R' enable watchdog that will result in an reset	308	// if 2nd byte is an 'R' enable watchdog that will result in an reset
309	if (rxd_buffer[2] == 'R') {	309	if (rxd_buffer[2] == 'R') {
310	wdt_enable(WDTO_250MS);	310	wdt_enable(WDTO_250MS);
311	} // Reset-Commando	311	} // Reset-Commando
312	} else { // checksum invalid	312	} else { // checksum invalid
313	rxd_buffer_locked = FALSE; // unlock rxd buffer	313	rxd_buffer_locked = FALSE; // unlock rxd buffer
314	}	314	}
315	ptr_rxd_buffer = 0; // reset rxd buffer pointer	315	ptr_rxd_buffer = 0; // reset rxd buffer pointer
316	}	316	}
317	} else { // rxd buffer overrun	317	} else { // rxd buffer overrun
318	ptr_rxd_buffer = 0; // reset rxd buffer	318	ptr_rxd_buffer = 0; // reset rxd buffer
319	rxd_buffer_locked = FALSE; // unlock rxd buffer	319	rxd_buffer_locked = FALSE; // unlock rxd buffer
320	}	320	}
321	}	321	}
322		322
323	// --------------------------------------------------------------------------	323	// --------------------------------------------------------------------------
324	void Addchecksum(uint16_t datalen) {	324	void Addchecksum(uint16_t datalen) {
325	uint16_t tmpchecksum = 0, i;	325	uint16_t tmpchecksum = 0, i;
326	for (i = 0; i < datalen; i++) {	326	for (i = 0; i < datalen; i++) {
327	tmpchecksum += txd_buffer[i];	327	tmpchecksum += txd_buffer[i];
328	}	328	}
329	tmpchecksum %= 4096;	329	tmpchecksum %= 4096;
330	txd_buffer[i++] = '=' + tmpchecksum / 64;	330	txd_buffer[i++] = '=' + tmpchecksum / 64;
331	txd_buffer[i++] = '=' + tmpchecksum % 64;	331	txd_buffer[i++] = '=' + tmpchecksum % 64;
332	txd_buffer[i++] = '\r';	332	txd_buffer[i++] = '\r';
333	txd_complete = FALSE;	333	txd_complete = FALSE;
334	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)	334	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
335	}	335	}
336		336
337	// --------------------------------------------------------------------------	337	// --------------------------------------------------------------------------
338	// application example:	338	// application example:
339	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);	339	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
340	/*	340	/*
341	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	341	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
342	va_list ap;	342	va_list ap;
343	uint16_t txd_bufferIndex = 0;	343	uint16_t txd_bufferIndex = 0;
344	uint8_t *currentBuffer;	344	uint8_t *currentBuffer;
345	uint8_t currentBufferIndex;	345	uint8_t currentBufferIndex;
346	uint16_t lengthOfCurrentBuffer;	346	uint16_t lengthOfCurrentBuffer;
347	uint8_t shift = 0;	347	uint8_t shift = 0;
348		348
349	txd_buffer[txd_bufferIndex++] = '#'; // Start character	349	txd_buffer[txd_bufferIndex++] = '#'; // Start character
350	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)	350	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)
351	txd_buffer[txd_bufferIndex++] = cmd; // Command	351	txd_buffer[txd_bufferIndex++] = cmd; // Command
352		352
353	va_start(ap, numofbuffers);	353	va_start(ap, numofbuffers);
354		354
355	while(numofbuffers) {	355	while(numofbuffers) {
356	currentBuffer = va_arg(ap, uint8_t*);	356	currentBuffer = va_arg(ap, uint8_t*);
357	lengthOfCurrentBuffer = va_arg(ap, int);	357	lengthOfCurrentBuffer = va_arg(ap, int);
358	currentBufferIndex = 0;	358	currentBufferIndex = 0;
359	// Encode data: 3 bytes of data are encoded into 4 bytes,	359	// Encode data: 3 bytes of data are encoded into 4 bytes,
360	// where the 2 most significant bits are both 0.	360	// where the 2 most significant bits are both 0.
361	while(currentBufferIndex != lengthOfCurrentBuffer) {	361	while(currentBufferIndex != lengthOfCurrentBuffer) {
362	if (!shift) txd_buffer[txd_bufferIndex] = 0;	362	if (!shift) txd_buffer[txd_bufferIndex] = 0;
363	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);	363	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);
364	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;	364	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
365	shift += 2;	365	shift += 2;
366	if (shift == 6) { shift=0; txd_bufferIndex++; }	366	if (shift == 6) { shift=0; txd_bufferIndex++; }
367	currentBufferIndex++;	367	currentBufferIndex++;
368	}	368	}
369	}	369	}
370	// If the number of data bytes was not divisible by 3, stuff	370	// If the number of data bytes was not divisible by 3, stuff
371	// with 0 pseudodata until length is again divisible by 3.	371	// with 0 pseudodata until length is again divisible by 3.
372	if (shift == 2) {	372	if (shift == 2) {
373	// We need to stuff with zero bytes at the end.	373	// We need to stuff with zero bytes at the end.
374	txd_buffer[txd_bufferIndex] &= 0b00110000;	374	txd_buffer[txd_bufferIndex] &= 0b00110000;
375	txd_buffer[++txd_bufferIndex] = 0;	375	txd_buffer[++txd_bufferIndex] = 0;
376	shift = 4;	376	shift = 4;
377	}	377	}
378	if (shift == 4) {	378	if (shift == 4) {
379	// We need to stuff with zero bytes at the end.	379	// We need to stuff with zero bytes at the end.
380	txd_buffer[txd_bufferIndex++] &= 0b00111100;	380	txd_buffer[txd_bufferIndex++] &= 0b00111100;
381	txd_buffer[txd_bufferIndex] = 0;	381	txd_buffer[txd_bufferIndex] = 0;
382	}	382	}
383	va_end(ap);	383	va_end(ap);
384	Addchecksum(pt); // add checksum after data block and initates the transmission	384	Addchecksum(pt); // add checksum after data block and initates the transmission
385	}	385	}
386	*/	386	*/
387		387
388	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	388	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
389	va_list ap;	389	va_list ap;
390	uint16_t pt = 0;	390	uint16_t pt = 0;
391	uint8_t a, b, c;	391	uint8_t a, b, c;
392	uint8_t ptr = 0;	392	uint8_t ptr = 0;
393		393
394	uint8_t *pdata = 0;	394	uint8_t *pdata = 0;
395	int len = 0;	395	int len = 0;
396		396
397	txd_buffer[pt++] = '#'; // Start character	397	txd_buffer[pt++] = '#'; // Start character
398	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)	398	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
399	txd_buffer[pt++] = cmd; // Command	399	txd_buffer[pt++] = cmd; // Command
400		400
401	va_start(ap, numofbuffers);	401	va_start(ap, numofbuffers);
402		402
403	if (numofbuffers) {	403	if (numofbuffers) {
404	pdata = va_arg(ap, uint8_t*);	404	pdata = va_arg(ap, uint8_t*);
405	len = va_arg(ap, int);	405	len = va_arg(ap, int);
406	ptr = 0;	406	ptr = 0;
407	numofbuffers--;	407	numofbuffers--;
408	}	408	}
409		409
410	while (len) {	410	while (len) {
411	if (len) {	411	if (len) {
412	a = pdata[ptr++];	412	a = pdata[ptr++];
413	len--;	413	len--;
414	if ((!len) && numofbuffers) {	414	if ((!len) && numofbuffers) {
415	pdata = va_arg(ap, uint8_t*);	415	pdata = va_arg(ap, uint8_t*);
416	len = va_arg(ap, int);	416	len = va_arg(ap, int);
417	ptr = 0;	417	ptr = 0;
418	numofbuffers--;	418	numofbuffers--;
419	}	419	}
420	} else	420	} else
421	a = 0;	421	a = 0;
422	if (len) {	422	if (len) {
423	b = pdata[ptr++];	423	b = pdata[ptr++];
424	len--;	424	len--;
425	if ((!len) && numofbuffers) {	425	if ((!len) && numofbuffers) {
426	pdata = va_arg(ap, uint8_t*);	426	pdata = va_arg(ap, uint8_t*);
427	len = va_arg(ap, int);	427	len = va_arg(ap, int);
428	ptr = 0;	428	ptr = 0;
429	numofbuffers--;	429	numofbuffers--;
430	}	430	}
431	} else	431	} else
432	b = 0;	432	b = 0;
433	if (len) {	433	if (len) {
434	c = pdata[ptr++];	434	c = pdata[ptr++];
435	len--;	435	len--;
436	if ((!len) && numofbuffers) {	436	if ((!len) && numofbuffers) {
437	pdata = va_arg(ap, uint8_t*);	437	pdata = va_arg(ap, uint8_t*);
438	len = va_arg(ap, int);	438	len = va_arg(ap, int);
439	ptr = 0;	439	ptr = 0;
440	numofbuffers--;	440	numofbuffers--;
441	}	441	}
442	} else	442	} else
443	c = 0;	443	c = 0;
444	txd_buffer[pt++] = '=' + (a >> 2);	444	txd_buffer[pt++] = '=' + (a >> 2);
445	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	445	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
446	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	446	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
447	txd_buffer[pt++] = '=' + (c & 0x3f);	447	txd_buffer[pt++] = '=' + (c & 0x3f);
448	}	448	}
449	va_end(ap);	449	va_end(ap);
450	Addchecksum(pt); // add checksum after data block and initates the transmission	450	Addchecksum(pt); // add checksum after data block and initates the transmission
451	}	451	}
452		452
453	// --------------------------------------------------------------------------	453	// --------------------------------------------------------------------------
454	void Decode64(void) {	454	void Decode64(void) {
455	uint8_t a, b, c, d;	455	uint8_t a, b, c, d;
456	uint8_t x, y, z;	456	uint8_t x, y, z;
457	uint8_t ptrIn = 3;	457	uint8_t ptrIn = 3;
458	uint8_t ptrOut = 3;	458	uint8_t ptrOut = 3;
459	uint8_t len = ReceivedBytes - 6;	459	uint8_t len = ReceivedBytes - 6;
460		460
461	while (len) {	461	while (len) {
462	a = rxd_buffer[ptrIn++] - '=';	462	a = rxd_buffer[ptrIn++] - '=';
463	b = rxd_buffer[ptrIn++] - '=';	463	b = rxd_buffer[ptrIn++] - '=';
464	c = rxd_buffer[ptrIn++] - '=';	464	c = rxd_buffer[ptrIn++] - '=';
465	d = rxd_buffer[ptrIn++] - '=';	465	d = rxd_buffer[ptrIn++] - '=';
466	//if(ptrIn > ReceivedBytes - 3) break;	466	//if(ptrIn > ReceivedBytes - 3) break;
467		467
468	x = (a << 2) \| (b >> 4);	468	x = (a << 2) \| (b >> 4);
469	y = ((b & 0x0f) << 4) \| (c >> 2);	469	y = ((b & 0x0f) << 4) \| (c >> 2);
470	z = ((c & 0x03) << 6) \| d;	470	z = ((c & 0x03) << 6) \| d;
471		471
472	if (len--)	472	if (len--)
473	rxd_buffer[ptrOut++] = x;	473	rxd_buffer[ptrOut++] = x;
474	else	474	else
475	break;	475	break;
476	if (len--)	476	if (len--)
477	rxd_buffer[ptrOut++] = y;	477	rxd_buffer[ptrOut++] = y;
478	else	478	else
479	break;	479	break;
480	if (len--)	480	if (len--)
481	rxd_buffer[ptrOut++] = z;	481	rxd_buffer[ptrOut++] = z;
482	else	482	else
483	break;	483	break;
484	}	484	}
485	pRxData = &rxd_buffer[3];	485	pRxData = &rxd_buffer[3];
486	RxDataLen = ptrOut - 3;	486	RxDataLen = ptrOut - 3;
487	}	487	}
488		488
489	// --------------------------------------------------------------------------	489	// --------------------------------------------------------------------------
490	void usart0_ProcessRxData(void) {	490	void usart0_ProcessRxData(void) {
491	// We control the motorTestActive var from here: Count it down.	491	// We control the motorTestActive var from here: Count it down.
492	if (motorTestActive)	492	if (motorTestActive)
493	motorTestActive--;	493	motorTestActive--;
494	// if data in the rxd buffer are not locked immediately return	494	// if data in the rxd buffer are not locked immediately return
495	if (!rxd_buffer_locked)	495	if (!rxd_buffer_locked)
496	return;	496	return;
497	uint8_t tempchar[3];	497	uint8_t tempchar[3];
498	Decode64(); // decode data block in rxd_buffer	498	Decode64(); // decode data block in rxd_buffer
499		499
500	switch (rxd_buffer[1] - 'a') {	500	switch (rxd_buffer[1] - 'a') {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c @ 1988 - Rev 1980 → 1986