WebSVN - FlightCtrl - Diff - Rev 1544 and 1545 - /branches/V0.76g


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (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 sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
 
#include "eeprom.h"
#include "main.h"
#include "menu.h"
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "fc.h"
#include "rc.h"
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
#include "ubx.h"
#endif
#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 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 PcAccess = 100;
 
uint8_t MotorTest_Active  = 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;
ExternControl_t ExternControl;
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
    "AngleNick       ", //0
    "AngleRoll       ",
    "AccNick         ",
    "AccRoll         ",
    "YawGyro         ",
    "Height Value    ", //5
    "AccZ            ",
    "Gas             ",
    "Compass Heading ",
    "Voltage         ",
    "Receiver Level  ", //10
    "YawGyro Heading ",
    "Motor Front     ",
    "Motor Rear      ",
    "Motor Left      ",
    "Motor Right     ", //15
    "                ",
    "                ",
    "VarioMeter      ",
    "MK3MAG CalState ",
    "NickServo       ", //20
    "Hoovergas       ",
    "FC_JN Input 1   ", // LPD: FC_JN_Receiver support added
    "FC_JN Input 2   ",
    "FC_JN Input 3   ",
    "FC_JN Input 4   ", //25
    "                ",
    "                ",
    "I2C-Error       ",
    "                ",
    "GPS Nick        ", //30
    "GPS Roll        "
};
 
 
 
/****************************************************************/
/*              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 (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
        // If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.
        if(CPUType != ATMEGA644P) ubx_parser(c);
        #endif
 
        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') // Reset-Commando
                                {
                                        wdt_enable(WDTO_250MS);
                                        Servo_Off();
                                }
                        }
                        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)
}
 
 
 
// --------------------------------------------------------------------------
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)
{
        // 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);
                                }
                                //Request_MotorTest = TRUE;
                                MotorTest_Active = 255;
                                PcAccess = 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 *) &ParamSet, sizeof(ParamSet));
                                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(&ParamSet, (uint8_t*)&pRxData[2], sizeof(ParamSet));
                                                ParamSet_WriteToEEProm(pRxData[0]);
                                                TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
                                                TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
                                                tempchar1 = GetActiveParamSet();
                                                LipoDetection(0); // low voltage warning
                                                Beep(tempchar1, 110);
                                        }
                                        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;
                                PcAccess = 255;
                                break;
 
                        case 'b': // submit extern control
                                memcpy(&ExternControl, (uint8_t*)pRxData, sizeof(ExternControl));
                                ConfirmFrame = ExternControl.Frame;
                                PcAccess = 255;
                                break;
 
                        case 'h':// request for display columns
                                PcAccess = 255;
                                RemoteKeys |= pRxData[0];
                                if(RemoteKeys) DisplayLine = 0;
                                Request_Display = TRUE;
                                break;
 
                        case 'l':// request for display columns
                                PcAccess = 255;
                                MenuItem = pRxData[0];
                                Request_Display1 = TRUE;
                                break;
 
                        case 'v': // request for version and board release
                                Request_VerInfo = 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;
        }
        else 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;
        }
        else if(Request_Display1 && txd_complete)
        {
                LCD_PrintMenu();
                SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
                Request_Display1 = FALSE;
        }
        else 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;
        }
        else if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
        {
                SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
                ConfirmFrame = 0;
        }
        else if( (((DebugData_Interval > 0) && 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;
    }
    else if( (((Data3D_Interval > 0) && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete)
        {
                SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
                Data3D.AngleNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D.AngleRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D.Heading   = (int16_t)((10 * YawGyroHeading)   / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D_Timer = SetDelay(Data3D_Interval);
                Request_Data3D = FALSE;
    }
        else if(Request_ExternalControl && txd_complete)
        {
                SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &ExternControl, sizeof(ExternControl));
                Request_ExternalControl = FALSE;
        }
 
        #ifdef USE_MK3MAG
    else if((CheckDelay(Compass_Timer)) && txd_complete)
        {
                ToMk3Mag.Attitude[0] = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
                ToMk3Mag.Attitude[1] = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
                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
 
        else if(Request_MotorTest && txd_complete)
        {
                SendOutData('T', FC_ADDRESS, 0);
                Request_MotorTest = FALSE;
        }
        else if(Request_PPMChannels && txd_complete)
        {
                SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in));
                Request_PPMChannels = FALSE;
        }
}
 
 

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

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_FC-JN-Receiver/uart0.c - Rev 1544 → 1545