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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c @ 2510 - Rev 2032 → 2033