WebSVN - FlightCtrl - Diff - Rev 1872 and 1874 - /branches/dongfang_FC


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

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c - Rev 1872 → 1874