WebSVN - FlightCtrl - Diff - Rev 2019 and 2026 - /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
                "UBat            ",
                "Pitch Term      ",
                "Roll Term       ",
                "Yaw Term        ",
                "Throttle Term   ", //15
                "gyroP           ",
                "ControlAct/10   ",
        "Acc. Vector     ",
        "targetheight    ",
                "var0            ", //20
                "dHeightThrottle ",
                "M1              ",
                "M2              ",
                "M3              ",
                "M4              ", //25
                "Height          ",
                "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
 
        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 2019	Rev 2026
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	"UBat ",
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	"Max. Acc. Vector",	143	"targetheight ",
144	"var0 ", //20	144	"var0 ", //20
145	"rc signal ",	145	"dHeightThrottle ",
146	"M1 ",	146	"M1 ",
147	"M2 ",	147	"M2 ",
148	"M3 ",	148	"M3 ",
149	"M4 ", //25	149	"M4 ", //25
150	"ControlYaw ",	150	"Height ",
151	"Airpress. Range ",	151	"Airpress. Range ",
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 @ 2159 - Rev 2019 → 2026